Apparatus and method for autonomous vehicle navigation using absolute data

ABSTRACT

A system and method for controlling the navigation of surface based vehicle uses a route that is obtained by manually driving the vehicle over the route to collect data defining the absolute position of the vehicle at various positions along the route. The collected data is smoothed to provide a consistent route to be followed. The smoothed data is subsequently used to automatically guide the vehicle over the route.

This is a Division of Ser. No. 07/487,980 filed Feb. 05, 1990.

BACKGROUND OF THE INVENTION

1. Field of the Invention

This invention relates to systems, which as used herein may includeapparatus and/or methods, for determining terrestrial positioninformation, and to systems for navigating an autonomous vehicle.

2. Related Art

There is presently under development a terrestrial position determiningsystem, referred to as the global positioning system (GPS), designatedNAVSTAR by the U.S. Government. In this system, a multitude of orbitingsatellites will be used to determine the terrestrial position ofreceivers on the Earth. In the planned system, there will be eightorbiting satellites in each of three sets of orbits, 21 satellites online and three spares, for a total of 24 satellites. The three sets oforbits will have mutually orthogonal planes relative to the Earth. Theorbits are neither polar orbits nor equatorial orbits, however. Thesatellites will be in 12-hour orbits. The position of each satellite atall times will be precisely known. The longitude, latitude, and altitudewith respect to the center of the Earth, of a receiver at any pointclose to Earth at the time of transmission, will be calculated bydetermining the propagation time of transmissions from at least four ofthe satellites to the receiver. The more satellites used the better. Acurrent constraint on the number of satellites is that the currentlyavailable receiver only has five channels.

Energy on a single carrier frequency from all of the satellites istransduced by the receiver at a point close to Earth. The satellitesfrom which the energy originated are identified by modulating thecarrier transmitted from each satellite with pseudorandom type signals.In one mode, referred to as the coarse/acquisition (C/A) mode, thepseudorandom signal is a gold code sequence having a chip rate of 1.023MHz; there are 1,023 chips in each gold code sequence, such that thesequence is repeated once every millisecond (the chipping rate of apseudorandom sequence is the rate at which the individual pulses in thesequence are derived and therefore is equal to the code repetition ratedivided by the number of members in the code; one pulse of the noisecode is referred to as a chip).

The 1.023 MHz gold code sequence chip rate enables the position of thereceiver responsive to the signals transmitted from four of thesatellites to be determined to an accuracy of approximately 60 to 300meters.

There is a second mode, referred to as the precise or protected (P)mode, wherein pseudorandom codes with chip rates of 10.23 MHz aretransmitted with sequences that are extremely long, so that thesequences repeat no more than once per week. In the P mode, the positionof the receiver can be determined to an accuracy of approximately 16 to30 meters. However, the P mode requires Government classifiedinformation about how the receiver is programed and is intended for useonly by authorized receivers. Hence, civilian and/or military receiversthat are apt to be obtained by unauthorized users are not responsive tothe P mode.

To enable the receivers to separate the C/A signals received from thedifferent satellites, the receiver includes a plurality of differentlocally derived gold code sources, each of which corresponds with thegold code sequence transmitted from one of the satellites in the fieldof the receiver. The locally derived and the transmitted gold codesequences are cross correlated with each other over one millisecond,gold code sequence intervals. The phase of the locally derived gold codesequences vary on a chip-by-chip basis, and then within a chip, untilthe maximum cross correlation function is obtained. Since the crosscorrelation for two gold code sequences having a length of 1,023 bits isapproximately 16 times as great as the cross correlation function of anyof the other combinations of gold code sequences, it is relatively easyto lock the locally derived gold code sequence onto the same gold codesequence that was transmitted by one of the satellites.

The gold code sequences from at least four of the satellites in thefield of view of the receiver are separated in this manner by using asingle channel that is sequentially responsive to each of the locallyderived gold code sequences or by using parallel channels that aresimultaneously responsive to the different gold code sequences. Afterfour locally derived gold code sequences are locked in phase with thegold code sequences received from four satellites in the field of viewof the receiver, the position of the receiver can be determined to anaccuracy of approximately 60 to 300 meters.

The approximately 60 to 300 meter accuracy of GPS is determined by (1)the number of satellites transmitting signals to which the receiver iseffectively responsive, (2) the variable amplitudes of the receivedsignals, and (3) the magnitude of the cross correlation peaks betweenthe received signals from the different satellites.

In response to reception of multiple pseudorange noise (PRN) signals,there is a common time interval for some of the codes to likely cause adegradation in time of arrival measurements of each received PRN due tothe cross correlations between the received signals. The time of arrivalmeasurement for each PRN is made by determining the time of a peakamplitude of the cross correlation between the received composite signaland a local gold code sequence that is identical to one of thetransmitted PRN. When random noise is superimposed on a received PRN,increasing the averaging time of the cross correlation between thesignal and a local PRN sequence decreases the average noise contributionto the time of arrival error. However, because the cross correlationerrors between the received PRN's are periodic, increasing the averagingtime increases both signal and the cross correlation value between thereceived PRN's alike and time of arrival errors are not reduced.

In addition to the GPS, it is known in the field to use inertial systemsto obtain user position estimates. Such an inertial reference unit (IRU)obtains specific-force measurements from accelerometers in a referencecoordinate frame which is stabilized by gyroscopes. An IRU may be of thelaser or mechanical type. In an unaided inertial navigation system, theaccelerometer measured specific force (corrected for the effects of theEarth's gravity) is integrated in a navigation equation to produce theuser's position and velocity.

The IRU instrument measurements may be specified in a differentrectangular coordinate frame than the reference navigation frame,depending on the platform implementation. The most commonly usedreference navigation frame for near Earth navigation is the local-levelframe (east-north-vertical). For a gimballed, local level-north seekingIRU, the gyros and accelerometers are mounted on a platform which istorqued to maintain the platform level and azimuth pointing to thenorth. For a gimballed, local-level azimuth-wander IRU, on the otherhand, the platform is maintained level but is not torqued about thevertical axis.

In a strap down mechanization of the IRU, on the other hand, theaccelerometers and gyros are directly mounted on the vehicle body. Theymeasure the linear and angular motion of the vehicle relative toinertial space, expressed in vehicle coordinates. It is, therefore,necessary in a strap down mechanization, to first compute numericallythe attitude of the vehicle to the referenced navigation frame, and thenuse the computed attitude to transform the accelerometer measurementsinto the reference frame. After the accelerometer data of a strap downIRU have been resolved into the reference frame, the solution of the IRUnavigation equations is identical in both the gimballed and strap downimplementation.

In the strap down IRU implementation, the attitude computations,required to resolve accelerometer measurements, are carried out at ahigh rate. They suffer from numerical errors because of the limitedcomputer word size and throughput availability. These computation errorsdepend on the frequency response of the sensor loop, data rate, andresolution and magnitude of the sensor output at the sampling time.There are significant benefits to a strap down IRU mechanization over agimballed platform implementation. The potential to realize size andcost reduction in the IRU makes consideration of strap down systemsattractive for both military and commercial applications.

The IRU navigation performance is primarily limited by its sensor errorsources. Uncertainties in gyro drift and accelerometers bias, scalefactor and IRU alignment angles are some of the significant errorsources which effect the accuracy of the inertial navigator. Typically,these error sources cause errors in the estimates of user position,velocity and attitude, which accumulate with time on a mission and are,to some extent, user dynamics dependent.

To support a requirement of very accurate navigation, high precisiongyros and accelerometers are required, which increase the complexity andcosts of the vehicle.

Rudimentary autonomous, meaning unmanned or machine controlled, vehiclenavigation is also known in the field.

Systems exist which rely on vision based positioning. For instance,vision based positioning is used in the Martin Marietta Autonomous LandVehicle, as described in "Obstacle Avoidance Perception Processing forthe Autonomous Land Vehicle," by R. Terry Dunlay, IEEE,CH2555-1/88/0000/0912$01.00, 1988. (See also "A Curvature-based Schemefor Improving Road Vehicle Guidance by computer Vision," by E. D.Dickmanns and A. Zapp, as reported at SPIE's Cambridge Symposium onOptical and Optoelectronic Engineering, Oct. 26-31, 1986.)

Some of these vision based positioning systems may use fixed lines ormarkings on, for instance, a factory floor, to navigate from point topoint. Others may involve complex pattern recognition hardware andsoftware. Other systems may navigate by keeping track of their positionrelative to a known starting point by measuring the distance they havetravelled and the direction from the starting point. These measuringtype systems are known as "dead-reckoning" systems.

Such navigation systems suffer from numerous drawbacks and limitations.For instance, if the system cannot recognize where it is, looses trackof where it has been, or miscalculates its starting point, it willbecome unable to accurately reach its goal. Because errors in positionaccumulate in such systems, they may require frequent, time consuminginitializations. Such systems may require patterns and markers be placedalong their route, which is also time consuming and costly, and limitstheir usefulness to small, controlled areas.

SUMMARY OF THE INVENTION

The present invention is an integrated vehicle positioning andnavigation system which, as used throughout, means apparatus, method ora combination of both apparatus and method. The present inventionovercomes many of the limitations present in conventional technology inthe fields of positioning and navigation, and thereby provides forhighly accurate and autonomous positioning and navigation of a vehicle.

In the present invention, the conventional capabilities of the IRU(Inertial Reference Unit) and the GPS (Global Positioning System) havebeen both combined, and greatly enhanced, in a cost-effective manner toprovide autonomous vehicle capability. In doing so, the presentinvention uses many novel and inventive systems, apparatus, methods andtechniques which allow for a superior positioning capability, andflexible autonomous navigational capability.

In the present invention, the vehicle positioning and navigation systemachieves some of its advantages by using a novel and enhancedcombination of three independent subsystems to determine position. Theseare: GPS (Global Positioning System) satellites, an inertial referencesystem, and the vehicle odometer. These three independent subsystemsproduce positioning data which may then be combined by a novel Kalmanfilter so that an output stream of positioning data of improved accuracymay be produced at a much faster rate (approximately 20 Hz).

The present invention also provides for increased positioning accuracyby providing novel systems for compensating for noise and errors inpositioning data.

The present invention also provides for anti-spoofing, that is detectingand correcting intentionally false positioning data transmitted by thesatellite positioning system.

The present invention also includes novel satellite position predictionsystems, further enhancing the accuracy and performance of thepositioning system.

This superior positioning data capability of the present invention, isused to aid the autonomous vehicle navigation system of the presentinvention. The navigation system of the present invention, provides forhighly accurate vehicle path-tracking on predetermined, or dynamicallydetermined, vehicle paths.

The present invention includes novel systems for determining paths orroads for an autonomous vehicle to follow at a work site, bothdynamically during operation and/or in advance. It includes systems forefficiently storing and accessing these paths. And it includes systemsfor accurately tracking these paths. The present invention alsopossesses the capability to travel autonomously without pre-set pathsfrom beginning to ending points.

The present invention contemplates a totally autonomous work site, wherenumerous autonomous vehicles and equipment may operate with minimalhuman supervision. This reduces labor costs. This also makes the presentinvention suitable for hazardous or remote work sites, such as at thesite of a nuclear accident, for example.

The present invention provides the capability for monitoring of vehicleand equipment control and mechanical systems, such as oil, air, and/orhydraulic pressures and temperatures. It provides for the indicating ofa problem in any one of its systems, and for the scheduling of regularand emergency maintenance.

The present invention also provides for efficient scheduling of work anddispatching of vehicles and equipment at an autonomous work site, forinstance, by providing a host processing system at a base station.

The host processing system coordinates various positioning andnavigation tasks. It also acts as the supervisory interface between theautonomous system and its human managers.

A preferred embodiment or "best mode" of the present invention, would bein an autonomous mining vehicle, such as is used for hauling ore, at amining site. However, it is noted that it is contemplated that thepresent invention may be implemented in other vehicles performing othertasks. Examples are, for instance, in other types of heavy equipment formining or construction, or in other totally different types of vehicles,such as farm tractors or automobiles.

The present invention includes novel systems for ensuring safe operationof a manned or unmanned autonomous vehicle. These may be embodied inmanual override features, failsafe features, error checking features,supervisory features, obstacle detection and avoidance, and redundancyfeatures.

The vehicle positioning and navigation system of the present inventionincludes systems which provide for a hierarchy of control such thatmanual control when asserted takes precedence as a safety feature.

The vehicle positioning and navigation system of the present inventionincludes systems which provide for a safe failure mode. This insuresthat should a system or subsystem malfunction, or drift unacceptably outof calibration, the system will detect this condition and bring thevehicle to a stop. The brake system, as an example, is designed with theparking brakes normally set. Any failure or loss of power will result inthe brakes being returned to their normal set mode, stopping the vehiclein the shortest distance possible.

The vehicle positioning and navigation system of the present inventionincludes systems which provide for error checking to detect, forinstance, garbled messages, or out-of-limit commands, which may indicatesystem malfunction.

The vehicle positioning and navigation system of the present inventionincludes systems which monitor and supervise the operation of othersystems and sub-systems to assure proper and safe operation. This mayinclude checking actual performance against a multi-state model of idealperformance of the vehicle and its systems.

The vehicle positioning and navigation system of the present inventionincludes systems for detecting obstacles in the vehicle path andenabling the vehicle to successfully avoid colliding with thoseobstacles, thereafter returning to the path. Such obstacles may befallen rocks, people, other vehicles, and so on.

The present invention also includes novel scanning systems for use indetecting obstacles. These may include scanner systems designed aroundlaser, sonar, radar, or other electromagnetic or acoustical waveapparatus.

The vehicle positioning and navigation system of the present inventionincludes redundancy of functional units, as backup and/or as checks onthe system operation.

The navigation portion of the present invention provides controlflexibility by providing for at least three modes of operation. Thesemodes include a manual mode, a remote control or "tele" mode, and afully autonomous mode. The "tele" mode may be over a remote radio, orcable, with the controlled vehicle in the line of sight of the "tele"operator. There is also a transitional or intermediate "ready" mode,used when going between certain other modes.

The present invention, in providing these modes, also provides foron-board, tele, and host displays for status and control information andmanipulation.

The present invention also includes novel vehicle control sub-systems,including speed control, steering control, and shutdown systems.

The present invention is able to achieve some of its advantages byproviding for vehicle control which serves to decouple speed andsteering control so that they may be controlled separately. Thissimplifies the problems that may be associated with accurate pathtracking, for instance.

The present invention also includes a novel global memory structure forcommunicating data between plural processing units, or tasks, and/orprocessors. This communication structure enables real-time,multi-tasking control of vehicle navigation.

A better appreciation of these and other advantages and features of thepresent invention, as well as how the present invention realizes them,will be gained from the following detailed description and drawings ofthe various embodiments, and from the claims.

BRIEF DESCRIPTION OF THE DRAWINGS

The present invention is better understood by reference to the followingdrawings in conjunction with the accompanying text.

FIG. 1 is a diagrammatic view of the GPS satellites in 6 orbital planes.

FIG. 2 is a diagrammatic view illustrating the GPS navigation equations.

FIG. 3 is a diagrammatic view of a typical autonomous work site in whichthe present invention is practiced.

FIG. 4 is a diagrammatic representation of the interrelationshipsbetween the navigator, the vehicle positioning system (VPS), and thevehicle controls.

FIG. 5 is a context diagram, illustrating the various elements and theirinterrelationship in an autonomous control system according to thepresent invention.

FIG. 6 is a diagrammatic representation of the operation of a GlobalPositioning System (GPS), including a satellite constellation, Apseudolite, the base station and a vehicle.

FIG. 7 is a block diagram representing the GPS Processing System.

FIG. 8 is a flow diagram illustrating communications in one embodimentof the GPS Processing System of FIG. 7.

FIG. 9 is a block diagram illustrating the Motion Positioning System(MPS) including the Inertial Reference Unit and the Odometer.

FIG. 10 is a block diagram illustrating one embodiment of the VPS SystemArchitecture.

FIG. 11 is a detailed block diagram of the embodiment of the VPS SystemArchitecture of FIG. 10.

FIG. 12 is a block diagram of one embodiment of the VPS Main Processorof FIG. 10 showing the VPS Kalman Filter and the Weighted Combiner.

FIG. 13 is a flowchart of the Constellation Effects Method for improvingposition accuracy.

FIG. 14 is a polar plot illustrating the computed pseudo ranges from afour satellite constellation.

FIG. 15 is a flowchart of the original bias technique for DifferentialCorrections.

FIG. 16 is a flowchart of the Parabolic Bias Method for DifferentialCorrections.

FIG. 17 is a flowchart of the BASE residuals as BIAS Method forDifferential Corrections.

FIG. 18 is a flowchart of the method for Satellite Position Prediction.

FIG. 19 is a flowchart of the Weighted Path History Method.

FIG. 20 is a diagrammatic representation of the Weighted Path HistoryMethod.

FIG. 21 is a flowchart of the Anti-Spoofing Method.

FIG. 22 is a diagram of route definitions using nodes and segmentsaccording to the present invention.

FIG. 23 is a diagrammatical representation of how postures andassociated circles are obtained from objective points.

FIG. 24 is a diagrammatical representation of how the sign of the firstclothoid segment is determined.

FIG. 25 is a diagrammatical representation of how the sign of the lastclothoid segment is determined.

FIG. 26 is a graphical illustration of a clothoid curve.

FIG. 27 is a flowchart of a numerical method for calculating approximateFresnel integrals.

FIG. 28 is a diagram showing the replanning of a path.

FIG. 29 is a graph of B-spline curves of second, third and fourth order.

FIG. 30 is a diagram of an embodiment of the posture ring buffer of thepresent invention.

FIG. 31 is a diagram of a path tracking control structure of anembodiment of the present invention.

FIG. 32 is a diagram showing relevant postures in steering planningcycle.

FIG. 33 is a diagram showing how an error vector including curvature iscomputed.

FIG. 34 is a diagram showing how an error vector including curvature iscomputed with the vehicle path included.

FIG. 35 is a context diagram of an embodiment of the navigator of thepresent invention.

FIG. 36 is a context diagram of a path tracking structure of the presentinvention.

FIGS. 37(A)-(D) are navigator data flow summaries.

FIG. 38A is an illustration of a vehicle mounted scanner.

FIG. 38B is an illustration of an autonomous vehicle scanning forobstacles.

FIG. 39 is a diagram of selected scan lines in an embodiment of a laserscanner system of the present invention.

FIG. 40 is a diagram of an autonomous vehicle avoiding obstacles.

FIG. 41 is a diagram of obstacle handling according to an embodiment ofthe present invention.

FIG. 42 is a schematic of a laser scanner system used for obstacledetection in an embodiment of the present invention.

FIG. 43 is a block diagram of an autonomous mining truck vehiclecontrols system of the present invention.

FIG. 44 is a state diagram showing the transitions between modes ofoperation.

FIG. 45 is a diagram of an embodiment of a tele-line of sight remotecontrol system of the present invention.

FIG. 46 is a representational diagram of an embodiment of the speedcontrol system of the present invention.

FIG. 47 is a diagram of an embodiment of the service brakes controlcircuit of the speed control system of the present invention.

FIG. 48 is a diagram of an embodiment of the governor control circuit ofthe speed control system of the present invention.

FIG. 49 is a diagram of an embodiment of a steering control circuit ofthe steering control system of the present invention.

FIG. 50 is a diagram of an embodiment of a park brake control circuit inthe speed control system of the present invention.

FIG. 51 is a diagram of a tricycle steering model used to developnavigation system of the present invention.

FIG. 52 is a diagram showing an embodiment of a shutdown circuit of thepresent invention.

FIG. 53 is a diagram showing navigator tasks.

FIG. 54 is a diagram showing an embodiment of navigator shared memory.

FIG. 55 is an executive flowchart.

FIG. 56 is an illustration of how FIGS. 56(a)-(d) relate.

FIGS. 56(A)-(D) together form a flowchart of the executive decisionsblock in detail.

FIGS. 57(A)-(R) are flowcharts each showing an "act on" block of theexecutive decisions flowchart in detail.

FIG. 58 is an illustration of how FIGS. 58(A)-(C) relate.

FIGS. 58(A)-(C) are parts of a flowchart showing the "act on state"block of the executive flowchart in more detail.

FIG. 59 is a diagram illustrating an old sensing and actuation timing.

FIG. 60 is a diagram illustrating the relationship between an inertialcoordinate system and a new coordinate system centered at the vehicle.

FIG. 61 is a diagram illustrating the relationship between a path andthe radius of curvature of the path.

FIG 62 is a diagram illustrating error versus speed.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT TABLE OF CONTENTS

I. General Overview

II. Vehicle Positioning System (VPS)

A. Overview

B. Global Positioning System (GPS)

1. GPS Space Segment

2. GPS System Operation

C. Motion Positioning System (MPS)

D. VPS System Architecture

E. Base Station

F. Satellite Based Accuracy Improvements

1. Constellation Effects

2. Differential Correction Techniques

a. Original Bias Technique

b. Parabolic Bias Technique

c. Base Residuals as Bias Technique

G. Satellite Position Predictor

H. Weighted Path History

I. Anti-Spoofing

J. Surveying

K. Graphic Representations

III. Navigation

A. Overview

B. Route Planning/Path Generation

1. Introduction

a. Clothoid Path Segments

b. Modeling A Vehicle Path

c. Clothoid Curves

d. Generation of a Posture Continuous Path

(1) Existing Methods

(2) Path generation from a sequence of points

(3) Clothoid Replanning Paths

(4) Summary

(5) B-splines

2. Route Creation and Storage

a. Introduction

b. Route Definition

c. Navigator Route Usage

3. Posture Generation

C. Path Tracking

1. Introduction

2. Considerations

a. Global position feed back

b. Separate steering and driving control

3. Embodiments

a. Tracking Control Structure

b. Quintic Method

c. Latency and slow systems

d. Vehicle-Ground Interaction (VGI)

e. Sensing and Actuation Timing

f. Look-ahead

g. Optimal Control Method

h. Conclusion

D. Obstacle Handling

1. Introduction

2. Detection of Obstacles

a. Clearance checking

b. Filtering and edge detection

c. Obstacle extraction

(1) Finding the road

(2) Modeling road height

(3) Thresholding

(4) Blob extraction

(5) Applications

3. Avoidance of Obstacles

4. Return to Path

5. Scanner System

a. Introduction

b. Laser scanner

c. Scanner system interface

d. Scanner system buffer circuit

E. Vehicle Controlling Systems

1. Introduction

2. Vehicle Manager (modes)

a. Ready mode

b. Tele mode

c. Manual mode

d. Autonomous mode

3. Speed Control

4. Steering Control

a. Steering Model

b. Path Representation

c. Posture Definition

d. Position Information

e. VPS Short Definition

f. Steering Method

5. Monitor/Auxiliary

6. Safety System

a. Introduction

b. Shutdown Control

7. Architectures

F. Functional Descriptions/Methods

1. The NAVIGATOR

a. MAIN

b. MONITOR₋₋ VEH STATUS

c. SCANNER

d. CONSOLE and CONSOLE₋₋ PARSER

e. GET₋₋ DIRECTIVES

f. MSG₋₋ TO₋₋ HOST

g. VPS₋₋ POSITION

h. VPS₋₋ POSTURE

i. TRACKER

j. NAVIGATOR Shared (Global) Memory

k. Flow Charts

I. General Overview

In the following description of the present invention, and throughoutthe specification, the term "system" is used for shorthand purposes tomean apparatus, method, or a combination of both apparatus and method.

Autonomous is used herein in its conventional sense. It indicatesoperation which is either completely automatic or substantiallyautomatic, that is, without significant human involvement in theoperation. An autonomous vehicle will generally be unmanned, that iswithout a human pilot or co-pilot. However, an autonomous vehicle may bedriven or otherwise operated automatically, and have one or more humanpassengers.

The task of guiding an autonomous vehicle along a prescribed pathrequires that one accurately knows the current position of the vehicle.Once the current position is known, one can command the vehicle toproceed to its next destination.

For a truly autonomous vehicle to be practical, very accurate positioninformation is required. In the past, it was not believed possible toobtain this nearly absolute position information without using aprohibitively large number of reference points. All positioning was donerelative to the last reference point using inertial navigation or deadreckoning. Using the systems of the present invention, we are able toobtain the nearly absolute position information required for a trulyautonomously navigated vehicle. This led us to the development ofsimpler, more reliable vehicle controllers and related systems as well.

The development and implementation of the GPS (global positioningsystem) was a necessary and vital link which allowed us to develop ourinventive systems to obtain more accurate position information. The GPSsatellite position information, significantly enhanced through ourinventive techniques, is combined and filtered with IRU (InertialReference Unit) inertial navigation information, and vehicle odometerinformation, according to one aspect of the present invention, resultingin a highly accurate system for determining position and effectingnavigation.

The present invention includes both positioning and navigation systems,apparatus, methods and techniques which, when integrated together,provide for the highly accurate transportation of unmanned vehicles.

The navigation system of the present invention, using the highlyaccurate position information obtained from the positioning system, isable to provide for accurate navigation between points alongpre-established or dynamically generated paths.

The present invention makes use of various models or conceptualrepresentations to generate and follows these paths. For example, linesand arcs may be used to establish paths between objective points thevehicle is to obtain. B-splines or clothoid curves may be used to modelthe actual path the vehicle is to navigate. Using modelling orrepresentational techniques provides for enhanced data communication,storage and handling efficiencies. It allows for simplification ofsupervisory tasks by providing a hierarchy of control and communicationsuch that the higher the level of control, the simpler the task, and themore compact the commands.

At the base station, where GPS positioning data is also received fromthe satellites, a host processing system provides for the highest levelof control of the navigation system. The host handles scheduling anddispatching of vehicles with much the same results as a human dispatcherwould achieve. The host thereby determines the work cycle of eachvehicle.

The host commands each of the vehicles to proceed from a currentposition to another position taking a specified route in order that theyeffect their work goals. The host specifies the routes by name, and notby listing each point along the route. Each vehicle has an on-boardsystem which looks up the named route and translates the named routeinto a set of nodes and segments along the route, using, for example,the models or representations discussed above to connect the nodes.

The on-board system also provides for controlling the vehicle systems,such as brakes, steering, and engine and transmission, to effect thenecessary physical acts required to move, stop, and steer the vehicle.These may be designed to be retrofitted onto existing vehicles, such asthe Caterpillar, Inc. 785 off-highway truck, for instance.

The on-board system also checks actual position against desired positionand corrects vehicle control to stay on route. The system may runmulti-state models to enhance this checking capability. The system alsochecks for errors or failures of the system and vehicle components. Ifdetected, the system provides for fail-safe shutdown, bringing thevehicle to a stop.

The on-board navigation system provides for different modes of control.These include (1) a fully autonomous mode, where navigation of thevehicle is automatically handled by the on-board system; (2) a tele orremote control mode, where a remote human operator may control thedirection and motion, and so on, of the vehicle; and (3) a manual mode,where a human operator sitting in the cab can take control of thevehicle and drive it manually.

In the autonomous mode, obstacle detection is critical, and is providedfor in the navigation system of the present invention. Boulders,animals, people, trees, or other obstructions may enter the path of avehicle unexpectedly. The on-board system is capable of detecting these,and either stopping, or plotting a path around the obstruction, toreturn to its original route when it is safe.

Accurately tracking the desired route is another function of theon-board navigational system. System function and architecture has beendesigned for real time tracking of the path at speeds up toapproximately 30 mph. safely.

As an introduction to the next section, "POSITIONING," recall that thevehicle positioning system (VPS) of the present invention is apositioning system based on the incorporation of Inertial Reference Unit(IRU) Data, Odometer Data and Global Positioning System (GPS) Data.

The VPS incorporates these three subsystems with extensive innovativemethodology to create a high accurate position determination system formoving or stationary vehicles, or any point on the Earth.

The GPS comprises a space segment and a land or atmospheric basedprocessing system. The space segment includes 24 special purposesatellites (not yet fully implemented) which are being launched andoperated by the U.S. Government. These satellites continually transmitdata to the Earth that can be read by a GPS receiver. The GPS receiveris part of a processing system which produces an estimate of the vehicleposition based on the transmitted data.

When multiple satellites are in view, the GPS receiver can read eachsatellite's navigation messages and determine position based on knowntriangulation methods. The accuracy of the process is in part dependenton how many satellites are in view. The IRU comprises laser gyroscopesand accelerometers which produce position, velocity, roll, pitch and yawdata. The IRU combines this information into an estimate of vehicleposition. The odometer produces data on distance travel that isincorporated with the IRU data.

A base station provides a geographic proximate reference point and readsthe data transmitted by each satellite. Using the transmitted data, thebase station makes improvements inaccuracy on the estimate of thevehicle position.

The present invention includes a method to predict the position of anysatellite at the current time or any future time. Using thatinformation, the GPS receiver can determine the optimum satelliteconstellation.

The present invention also includes a method for reducing vehicle"wandering" by using the path history of the vehicle to statisticallydetermine the accuracy of future estimates of position.

The present invention also includes numerous methods for determining howaccurate the data transmitted from the satellite is. Included in thesemethods are techniques to compensate for data with error.

II. Positioning

A. Overview

Referring now to FIGS. 7 through 11, the present invention includes aVehicle Positioning System (VPS) 1000 which is a highly accurateposition determination system. The VPS incorporates three subsystems andextensive innovative methodology to create an unsurpassed positiondetermination system for moving or stationary vehicles 310.

The VPS includes global positioning system (GPS) 700 and a motionpositioning system (MPS) 900 including an inertial reference unit (IRU)904, and an odometer 902 which have been enhanced and combined toproduce a highly effective position determining system. The GPS 700comprises a space segment and a land or atmospheric (for example, onelocated on an aircraft) based processing system. The space segmentincludes 24 special purpose satellites (not yet fully implemented; seeFIG. 1) which are being launched and operated by the U.S. Government.These satellites constantly transmit data to the Earth that is read by aGPS receiver 706.

The GPS receiver 706 is part of a GPS processing system 700 whichproduces an estimate of the vehicle position based on the transmitteddata. The vehicle's estimated position is transmitted to a VPS mainprocessor 1004.

When multiple satellites are in view (see FIG. 3), the GPS receiver 706can read each of their navigation messages and compute the position ofthe antenna 702 using triangulation methods. The accuracy of the processis in part dependent on how many satellites are in view. Each satellitein view increases the accuracy of the process.

The IRU 904 comprises laser gyroscopes and accelerometers which produceposition, velocity, roll, pitch, and yaw data. The IRU 904 combines thisinformation into an estimate of the vehicle position. The odometer 902produces distance traveled. The data from the IRU 904 and the odometer902 are also transmitted to the VPS main processor 1004.

The VPS main processor 1004 combines the data from the MPS 900 (the IRU904 and the odometer 902) with the estimated position from the GPSprocessing system 700 to produce a more accurate position.

Referring now to FIG. 3, a base station 314 provides a geographicproximate reference point for the VPS 1000. The base station 314includes its own GPS receiver 706 for reading the data transmitted bythe satellites. Using the transmitted data, the base station 314 allowsthe VPS 1000 to make satellite based accuracy improvements on theestimated of the vehicle position.

The present invention includes a method (as shown in the flowchart ofFIG. 18) to predict the position of any satellite at the current time orany future time. Given an estimate of the position of the GPS antenna702, the GPS receiver 706 can predict which satellites it will be ableto read at any time. Using that information, the GPS receiver 706 candetermine the optimum satellite constellation.

The present invention also includes a method for reducing vehicle"wandering" and spurious position computations. In this method, the pathhistory of the vehicle is used to statistically determine the accuracyof future estimates.

The present invention also includes a method for determining if the datareceived from the satellites is valid. The GPS receiver 706 determinesthe distance from the satellite to the GPS receiver 706 based on thedata being transmitted by each satellite. The GPS receiver 706 comparesthis determined distance with an expected distance based on the time andan estimated position. If the distances are within a given range, thenthe data from satellites is assumed to be valid. Otherwise, the data isadjusted such that the determined distance between the satellite and theGPS receiver is within the given range.

B. Global Positioning System (GPS)

1. GPS Space Segment

As shown in FIG. 1, 24 man-made electronic satellites which make up theglobal positioning system (GPS) are planned for deployment by the early1990s. As currently envisioned, the satellites will orbit the Earth atan altitude of approximately 10,900 miles and encircle the globe twice aday. With this conventional GPS system, it will be possible to determineterrestrial position within 15 meters in any weather, any time, and mostareas of the Earth.

As of the date of the filing of this application, there are known to beseven experimental GPS satellites in orbit, and several manufacturersthat are building GPS receivers. Until additional satellites aredeployed and operational, there are only two windows each day when threeor more satellites are available for position tracking. The location ofthese satellites (and all others once deployed) is very predictable andcan be plotted in terms of elevation and azimuth.

Reference is again made to FIG. 1 of the drawings wherein theconfiguration of the fully operational GPS system is schematicallyillustrated. Twenty-four (twenty-one operational, three spare) mediumorbiting satellites in six sets of orbits continuously transmit uniqueidentifying signals on a common carrier frequency.

Each of the 24 satellites transmits a unique navigation signal which canbe used to determine the terrestrial position of an Earth antennasensitive to the signal. The navigation signal is comprised of a databit stream which is modulated with a pseudo-random type binary codewhich biphase modulates the carrier frequency.

In the coarse acquisition (C/A) mode, each satellite has an establishedunique pseudo-random code, which is a gold code sequence having a lengthof 1,023 chips that repeats itself once every millisecond. To facilitateseparation of different satellite's signals, the gold code sequence foreach satellite has a low correlation with the gold code sequence fromthe other satellites.

In the carrier mode, the data transmitted by the satellites is encodedon the carrier frequency in a manner similar to the C/A code. In thecarrier mode, more data can be encoded and therefore more preciseposition computations can be made.

A GPS receiver can decode a navigation signal whose gold code sequencehighly correlates to an identical gold code sequence generated by theGPS receiver. The separation of the different satellite's signals from acommon carrier is based on cross correlation of the signal with thelocally generated gold code sequences, on a chip by chip basis, and thenwithin a chip, until the maximum cross correlation value is obtained;the gold code sequence which maximizes this value can be used to extractthe navigation data which is used in determining position of thereceiving antenna.

Turning now to FIG. 2, a diagrammatic representation of the GPS inoperation is shown. Four satellites 200, 202, 204, 206 comprise thecurrent group (constellation) of satellites in the view of the Earthantenna (user 210).

As is shown in the description block 208, each satellite is transmittinga navigation signal that includes timing (GPS time) and ephemeris(satellite position) data. Using the navigation equations 212, which arewell-known in the art, and the timing and ephemeris data from thenavigation signal, the position of the user 210 can be determined.

2. GPS System Operation

Turning now to FIG. 6, a representative GPS system is shown inoperation. Four GPS satellites 200, 202, 204 and 206 are transmittingnavigation signals in which satellite position (ephemeris) data andtransmission time data are encoded. Both a vehicle 310 and a basestation 314 are receiving these signals from each of these satellites ontheir respective GPS antennas 312 and 316. In a preferred embodiment,both the C/A code and the carrier frequency are received at GPS antennas312 and 316 for processing.

In addition to the four GPS satellites shown in the FIG. 6, a radiotransmitter 624 is also depicted. This radio transmitter 624 is commonlyknown as a pseudolite. These pseudolites, in one embodiment of thepresent invention, can be strategically placed around the perimeter of amine pit and emulate the GPS satellites as shown in FIG. 6. Thisarrangement can be extremely useful in situations such as a mine pit ormine shaft, in which mining vehicles may be out of view of one or moreof the GPS satellites, because of topographic features such as high minepit walls. These ground based pseudolites provide additional rangingsignals and can thus improve availability and accuracy of thepositioning capability in the present invention.

The pseudolites are synchronized to the GPS satellites and have a signalstructure that while may be different are compatible with the GPSsatellites. Note that when ranging to pseudolites, the ranging errordoes not include selective availability nor ionospheric errors. However,other errors must be accounted for such as tropospheric, pseudoliteclock error and multipath errors.

In a deep pit surface mining operation, the view of the sky is limitedby the walls of the mine and an inadequate number of satellites may bein view for position determination. In such a case in the presentinvention, one or more pseudolites would be placed on the rim of themine and used with a vehicle with the visible satellites to obtainaccurate vehicle position.

Transmission channel 618 represents the electromagnetic communicationschannel linking the base station 314 and the vehicle 310. Thetransmission channel 618 is used to transfer data between the basestation 314 and the vehicle 310. This transmission channel 618 isestablished by data-radios 620 and 622 which are transmitter/receivers.

The data-radios 620 and 622 are located at the base station 314 andvehicle 310 respectively, and are responsible for transferring variousdata between the base station 314 and the vehicle 310. The type of datatransferred will be discussed further below. No physical medium (forexample, copper wire, optical fiber) is necessary to conduct thetransmission of data.

A radio transmitter/receiver which functions appropriately with thepresent invention can be found in the art. Such a preferred radiotransmitter/receiver is commercially available from Dataradio Ltd. ofMontreal, Canada, Model Number DR-4800BZ.

Turning now to FIG. 7, a preferred embodiment of a GPS processing system700 is shown. The GPS processing system 700 on the vehicle 310 includesa GPS antenna 702. In a preferred embodiment, the GPS antenna 702 isreceptive to the radio spectrum of electro-magnetic radiation. However,the present invention contemplates reception of any signal by which GPSsatellites might encode the navigation data.

An antenna receptive to the radio spectrum used by GPS satellites whichfunctions satisfactorily with the present invention can be found in theart. Such a preferred antenna is commercially available from ChuAssociates Inc. of Littleton, Mass., Model # CA3224.

The GPS antenna 702 is coupled to a pre-amplifier 704 so that thesignals received at the GPS antenna 702 can be transmitted to thepre-amplifier 704. The present invention contemplates any method bywhich the GPS antenna 702 can be satisfactorily coupled to thepre-amplifier 704. Further, as will be noted at numerous places below,all devices in the GPS processing system 700, MPS processing system 900and VPS processing system 1000 are coupled. The present inventioncontemplates any method by which these devices can be satisfactorilycoupled.

Such coupling methods may include for example, electronic, optic, andsound methods as well as any others not expressly described herein. In apreferred embodiment, the coupling method used is electronic and adheresto any one of numerous industry standard interfaces.

The pre-amplifier 704 amplifies the signal received from the GPS antenna702 so that the signals can be processed (decoded). The presentinvention contemplates any method by which the received signals can beamplified.

A pre-amplifier which functions satisfactorily with the presentinvention can be found in the art. Such a preferred pre-amplifier iscommercially available from Stanford Telecommunications Inc. (STel) ofSanta Clara, Calif., Model Number 5300, Series GPS RF/IF.

The pre-amplifier 704 is coupled to a GPS receiver 706. The GPS receiver706 decodes and processes the navigation message sent from thesatellites in the view of the GPS antenna 702. During this processing,the GPS receiver 706 computes the latitude, longitude, and altitude ofall satellites in the particular constellation being viewed. The GPSreceiver 706 also computes pseudoranges, which are estimates of thedistances between the satellites in the currently viewed constellationand the GPS antenna 702. In a preferred embodiment, the GPS receiver 706can process, in parallel, pseudoranges for all satellites in view.

In a preferred embodiment of the present invention, the GPS receiver 706produces this data when four or more satellites are visible. In apreferred embodiment of the present invention, the GPS processing system700 can compute a position having an accuracy of approximately 25 meterswhen an optimal constellation of 4 satellites is in view. In anotherpreferred embodiment of the present invention, the GPS processing system700 can compute a position having an accuracy of approximately 15 meterswhen an optimal constellation of 5 satellites is in view. An optimalconstellation is one in which the relative positions of the satellitesin space affords superior triangulation capability, triangulationtechnology being well known in the art.

In a preferred embodiment of the present invention, the GPS receiver 706encodes in the outputted position data the number of satellitescurrently being viewed for each position computation made. In cases inwhich the number of satellites viewed for a series of positioncomputations is four, the VPS weighted combiner 1204 (see FIG. 12 anddiscussion) uses only the first position computation received in theseries. All subsequent position computations in the series are ignoredby the VPS weighted combiner 1204. The VPS weighted combiner 1204 actsin this manner because position computations derived from foursatellites are less accurate than nominally acceptable.

A receiver that functions satisfactorily in the present invention can befound in the art. Such a receiver is commercially available fromStanford Telecommunications Inc., Model Number 5305-NSI.

The GPS receiver 706 is coupled to a GPS inter communication processor708. The GPS inter communication processor 708 is also coupled to a GPSprocessor 710 and a GPS Console 1 712. The GPS inter communicationprocessor 708 coordinates data exchange between these three devices.Specifically, The GPS inter communication processor 708 receivespseudorange data from the GPS receiver 706 which it passes on to the GPSprocessor 710. The GPS inter communication processor 708 also relaysstatus information regarding the GPS receiver 706 and the GPS processor710 to the GPS Console 1 712.

An inter communication processor that functions satisfactorily with thepresent invention can be found in the art. Such a preferred intercommunication processor is commercially available from Motorola ComputerInc. of Cupertino, Calif., Model Number 68000.

The GPS processor 710 is passed the satellite location and pseudorangedata from the GPS inter communication processor 708. Turning now to FIG.8, the operation of the GPS processor is depicted. The GPS processor 710uses methods to process this data including a GPS kalman filter 802 (seeFIG. 8) which filters out noise buried in the pseudorange data,including ionospheric, clock, and receiver noise. The GPS kalman filter802 also reads biases (discussed further below) transmitted by the basestation 314 to the GPS processor 710.

Processor hardware that functions satisfactorily with the presentinvention as the GPS processor 710 can be found in the art. Suchhardware is commercially available from Motorola Computer Inc., ModelNumber 68020. The software in the GPS processor 710, in a preferredembodiment, functions in the following way.

In a preferred embodiment, this GPS kalman filter 802 is semi-adaptiveand therefore automatically modifies its threshold of acceptable dataperturbations, depending on the velocity of the vehicle 310. Thisoptimizes system response and accuracy of the present invention asfollows.

Generally, when the vehicle 310 increases velocity by a specifiedamount, the GPS kalman filter 802 will raise its acceptable noisethreshold. Similarly, when the vehicle 310 decreases its velocity by aspecified amount the GPS kalman filter 802 will lower its acceptablenoise threshold. This automatic optimization technique of the presentinvention provides the highest degree of accuracy under both moving andstationery conditions.

In the best mode of the present invention, the threshold of the GPSkalman filter 802 does not vary continuously in minute discreetintervals. Rather, the intervals are larger and, therefore, lessaccurate than a continuously varying filter. However, the kalman filter802 of the present invention is easy to implement, less costly andrequires less computation time than a continuously varying filter.However, such a continuously varying filter could be used.

The GPS kalman filter 802 must be given an initial value from the userat system start-up. From this value, and data collected by the GPSreceiver 706, the GPS kalman filter 802 extrapolates the current state(which includes position and velocities for northing, easting andaltitude) to the new "expected" position. This extrapolated position iscombined with new GPS data (an update) to produce the a current state.The way the data is utilized is dependent on an a priori saved filecalled a control file (not shown). This file will determine (1) how muchnoise the system is allowed to have, (2) how fast the system shouldrespond, (3) what are the initial guesses for position and velocity, (4)how far off the system can be before a system reset occurs, (5) how manybad measurements are allowed, and/or (6) how much time is allottedbetween measurements.

The GPS processor 710 then computes position, velocity, and time usingthe filtered data and biases. However, the GPS processor 710 discardsthe computed velocity datum when the C/A code rather than the carrier isprocessed by the GPS receiver 706 because experimentation has shown thatthis datum is not accurate.

Velocity data derived from the carrier frequency is not discardedbecause it is much more accurate than the C/A code velocity data. Thecomputed position and time data (and velocity data if derived from thecarrier frequency) are encoded on GPS Signal 716 and sent on to the VPSmain processor 1004 shown on FIG. 10.

In a preferred embodiment of the present invention, the GPS processor710 reads both of these codes depending on the availability of each.Unlike data transmitted using the C/A code, the carrier frequency datatransmitted by the satellite is available from the GPS receiver 706 atapproximately 50 hz (not approximately 2 hz.) This increased speedallows the present invention to produce more precise positiondeterminations with less error.

A preferred embodiment of other functions of the GPS processor 710 isshown in FIG. 8. However, the present invention contemplates any methodby which data transmitted by GPS satellites can be processed. As shownat a block 816, a console function controls the operation of the GPSconsole 2. This console function regulates the operation of the GPSkalman filter 802 by providing the user interface into the filter.

The VPS communications function shown at a block 818, controls theoutputs of the GPS kalman filter 802 which are directed to the VPSsystem 1000. At a block 806, it is shown that the GPS kalman filter 802requests and decodes data from the GPS receiver 706, which data isrouted through an IPROTO function shown at a block 804. The IPROTOfunction is not a function of the GPS processor, as indicated on FIG. 8by the dashed line. Rather, the IPROTO function resides on the GPS intercommunications processor 708 and executes tasks associated with the GPSinter communications processor 708. An IPROTO function that operatessatisfactorily with the present invention can be found in the art. Onemodel is commercially available from Xycom Inc., model number XVME-081.

As shown at a block 810 the data transmitted over the transmissionchannel 618 is decoded and transmitted into the GPS kalman filter 802.The communications manager function shown at a block 808, coordinatesthe incoming data from the IPROTO function. The communications mangerfunction 808 also coordinates data received from an ICC function whichis shown in a block 812. The ICC function 812 exchanges data with thedata-radio 714 and the GPS data collection device 718 as shown.

The GPS console 1 712, is well known in the art. Many types of devicesare commercially available which provide the desired function. One suchdevice is commercially available from Digital Equipment Corporation ofMaynard, Mass. Model Number VT220. The GPS Console 1 712 displaysprocessor activity data regarding GPS inter communication processor 708,and GPS processor 710.

The GPS processor 710 is coupled to a GPS Console 2 722 and a GPScommunications interface processor 720. The GPS console 2 722, is wellknown in the art. Many types of devices are commercially available whichprovide the desired console function. One such device is commerciallyavailable from Digital Equipment Corporation of Maynard, Mass. ModelNumber VT220. The GPS console 2 722 provides the user interface fromwhich the GPS processor 710 can be activated and monitored.

The GPS communications interface processor 720 is coupled to adata-radio 714 and a GPS data collection device 718. The GPScommunications interface processor 720 coordinates data exchange betweenthe GPS processor 710 and both the data-radio 714 and the GPS datacollection device 718. A communications interface processor whichfunctions appropriately can be found in the art. A preferredcommunications interface processor is commercially available fromMotorola Computer Inc., Model Number MVME331.

The data-radio 714 communicates information from the GPS processor 710(through the GPS communications interface processor 720) to a similardata-radio 620 located at the base station 314 (see FIG. 6). In apreferred embodiment, the data-radio 714 communicates synchronously at9600 baud. These data-radios provide periodic updates on the amount ofbias (as detected by the base station 314) are transmitted to thevehicle 310 at a rate of twice a second. Base station 314 computed biaswill be discussed further below.

The GPS data collection device 718 can be any of numerous commonelectronic processing and storage devices such as a desktop computer.The International Business Machines Corporation (IBM) PC available fromIBM of Boca Raton, Fla. can be used.

C. Motion Positioning System (MPS)

In a preferred embodiment, the present invention also includes thecombination of inertial reference unit (IRU) 904 and odometer 902components. These together with a processing device 906 comprise themotion positioning system (MPS) 900. IRUs and odometers are well knownin the field, and are commercially available from Honeywell Inc. ofMinneapolis, Minn., Model Number H61050-SR01, and from Caterpillar Inc.of Peoria, Ill., Part Number 7T6337 respectively.

Turning now to FIG. 9, a preferred embodiment of the motion positioningsystem 900 is depicted. A Vehicle odometer 902 and an IRU 904 arecoupled to the MPS inter communications processor 906.

The IRU 904 comprises laser gyroscopes and accelerometers of knowndesign. An IRU which can satisfactorily be used in the present inventionis a replica of the system used by Boeing 767 aircrafts to determineposition, except that the system used in the present invention has beenmodified to account for the lesser dynamics (for example, velocity) thatthe vehicles of the present invention will be exhibiting compared tothat of a 767 aircraft.

In a preferred embodiment, the IRU 904 outputs position, velocity, roll,pitch, and yaw data at rates of 50 hz (fifty (50) times a second); thevehicle odometer 902 outputs distance traveled at 20 hz.

The laser gyroscopes of the IRU 904, in order to function properly, mustbe given an estimate of vehicle latitude, longitude and altitude. Usingthis data as a baseline position estimate, the gyroscopes then use apredefined calibration in conjunction with forces associated with therotation of the earth to determine an estimate of the vehicle's currentposition.

This information is then combined by the IRU 904 with data acquired bythe IRU 904 accelerometers to produce a more accurate estimate of thecurrent vehicle position. The combined IRU 904 data and the vehicleodometer 902 data are transmitted to the MPS inter communicationsprocessor 906.

The MPS inter communications processor 906 forwards the IRU and odometerdata on to the VPS I/O processor 1002 (see FIG. 10), as shown at signals910 and 908, respectively.

An inter communications processor that functions satisfactorily with thepresent invention can be found in the art. Such a preferred intercommunications processor is commercially available from MotorolaComputer Inc., Model Number 68000.

The present invention contemplates any method by which the signals 716,908 and 910 can be received by the VPS I/O processor 1002 from the GPSsystem 700 and MPS system 900 and forwarded on to the VPS main processor1004. An I/O processor which functions satisfactorily with the presentinvention can be found in the art. Such an I/O processor is commerciallyavailable from Motorola Computer Inc., Model Number 68020.

D. Vehicle Positioning System (VPS) Architecture

Turning now to FIG. 10, a preferred embodiment of the VPS systemarchitecture 1000 is depicted. FIG. 11 shows a diagram of the same VPSsystem architecture 1000, with the GPS processing system 700 and MPSprocessing system 900 in detail.

GPS processing system 700 and MPS processing system 900 areindependently coupled to the VPS I/O processor 1002. Because they areindependent, the failure of one of the systems will not cause the otherto become inoperative. Thus, if the GPS processing system 700 is notoperative, data will still be collected and processed by the MPSprocessing system 900 and the VPS 1000. GPS processing system 700 andMPS processing system 900 transmit signals 716, 908, 910 to the VPS I/Oprocessor 1002, as shown. These signals contain position, velocity,time, pitch, roll, yaw, and distance data (see FIGS. 7 and 9 andassociated discussions).

The VPS I/O processor 1002 is coupled to the VPS main processor 1004.The VPS I/O processor 1002 transmits signals 1006, 1008, and 1010 to theVPS main processor 1004, as shown. These signals contain the GPS, IRUand odometer data noted above.

Turning now to FIG. 12, a preferred embodiment of the operation of theVPS main processor 1004 is depicted. As shown, the GPS signal 1006, andthe odometer signal 1008 are transmitted into a weighted combiner 1204.The IRU signal 1010 is transmitted into a VPS kalman filter 1202. In apreferred embodiment, the GPS signal 1006 is transmitted at a rate of 2Hz.; the Odometer signal 1008 is transmitted at a rate of 20 hz.; theIRU signal 1010 is transmitted at a rate of 50 hz.

The VPS kalman filter 1202 processes the IRU signal 1010 and filtersextraneous noise from the data. The VPS kalman filter 1202 also receivesa signal from the weighted combiner 1204, as shown, which is used toreset the VPS kalman filter with new position information.

The weighted combiner 1204 processes the signals and gives apredetermined weighing factor to each datum based on the estimatedaccuracy of data gathering technique used. Thus, in the best mode of thepresent invention, the position component of the GPS signal 1006 isweighted heavier than the position component of the IRU signal 1010.This is because GPS position determination is inherently more accuratethan IRU position determination.

However, velocity can be more accurately determined by the IRU, andtherefore the IRU velocity component is weighted heavier than the GPSvelocity component in the best mode of the invention.

The weighted combiner 1204 produces two outputs. One output contains allcomputed data and is sent to two locations: as shown at an arrow 1206,to the VPS kalman filter 1202; and as shown at the arrow 1016, out ofthe VPS main processor 1004. The second output shown at the arrow 1018contains only velocity data and is sent out of the VPS main processor1004 to the GPS processing system 700. The output shown at the arrow1016 contains GPS time, position, velocity, yaw, pitch, and roll dataand is transmitted at a rate of 20 hz.

The present invention contemplates any method by which the signals 1006,1008, and 1010 can be processed at the VPS main processor 1004 inaccordance with the above noted process steps. Processor hardware thatfunctions satisfactorily with the present invention as the VPS mainprocessor can be found in the art. Such hardware is commerciallyavailable from Motorola Computer Inc., Model Number 68020. The softwarein the VPS main processor 1004, in a preferred embodiment, functions asdescribed above.

Referring now back to FIG. 10, the VPS main processor 1004 is coupled toa VPS communications interface processor 1020.

A communications interface processor which functions satisfactorily withthe present invention can be found in the art. One preferred model iscommercially available from Motorola Computer Inc., Model NumberMVME331.

In a preferred embodiment, the VPS communications interface processor1020 is coupled to, and routes the data contained in Output 1016 (at 20hz.) to, three different devices: (1) a VPS console 1012, (2) a datacollection device 1014, and (3) a navigation system 1022.

The VPS console 1012 is well known in the art, and is commerciallyavailable from Digital Equipment Corporation, of Minneapolis, Minn.,Model Number VT220. This VPS console 1012 is used to display the currentstatus of the VPS main processor 1004.

The VPS data collection device 1014 can be any of numerous commonelectronic processing and storage devices such as a desktop computer.The MacIntosh computer available from Apple Computer of Cupertino,Calif., can be used successfully.

The navigation system 1022 comprises the features associated with thenavigational capabilities of the present invention. The VPS system 1000transmits its final data regarding vehicle position etc. to thenavigation system 1022 at this point.

E. Base Station

Referring back to FIG. 7, the present invention includes GPS componentsat the base station 314 that are identical to those which comprise theGPS processing system 700 (as shown on FIG. 7). The purpose of the basestation 314 is to (1) monitor the operation of the vehicles, (2) providea known terrestrial reference location from which biases can beproduced, and (3) provide information to the vehicles when necessary,over a high-speed data transmission channel 618.

In a preferred embodiment of the present invention, the base station 314will be located within close proximity to the vehicle 310, preferablywithin 20 miles. This will provide for effective radio communicationbetween the base station 314 and the vehicle 310 over the transmissionchannel 618. It will also provide an accurate reference point forcomparing satellite transmissions received by the vehicle 310 with thosereceived by the base station 314.

A geographically proximate reference point is needed in order to computeaccurate biases. Biases are, in effect, the common mode noise thatexists inherently in the GPS system. Once computed at the base station314, these biases are then sent to the vehicle 310 using the data-radios714 (as shown in FIG. 7). The biases are computed using various methodswhich are discussed further below.

In a preferred embodiment of the present invention, a host processingsystem 402 is located at the base station 314 to coordinate theautonomous activities and interface the VPS system 1000 with humansupervisors.

F. Satellite Based Accuracy Improvements

The present invention improves the position determining accuracy of thevehicle positioning system 1000 capability through a number ofdifferential techniques. These techniques are designed to remove errors(noise) from the pseudoranges that are calculated by the GPS receiver706. The removal of this noise results in more precise positiondetermination.

In a preferred embodiment, the base station 314 GPS processing system700 is responsible for executing these differential techniques. The term"differential" is used because the base station 314 and the vehicle 310use independent but virtually identical GPS Systems 700. Because thebase station 314 is stationary and its absolute position is known, itserves as a reference point from to measure electronic interference(noise) and other error inducing phenomena.

1. Constellation Effects

FIG. 13 depicts a flowchart which show the steps required to make thebest use of the satellite constellations which are in view of the GPSantenna 702. For a given vehicle 310, there may be numerous satellitesin view of which only a subset will form a particular constellation. Theconstellation effects flowchart 1300, describes how the optimalconstellation can be chosen depending on the satellites currently inview, and the desired path that the vehicle 310 is taking.

The beginning of the constellation effects flowchart 1300 is shown at ablock 1302. The first step in the flowchart is shown at block 1304. Thepositions of each satellite in view of the GPS antenna 702 thesatellites positions are computed using almanac data and GPS time.Almanac data refers to previously recorded data which stores GPSsatellites' positions at specific times during the day. Because theoperators of the GPS satellites know the course and track across the skythat the satellites take, this information is available.

The positions of the satellites are represented by polar coordinateswhich correspond to azimuth and elevation from the base station 314.Once these satellite positions have been determined, the satellites arethen mapped into a polar map with the estimated vehicle position in thecenter of the map. This polar map is illustrated in FIG. 14. As shown inFIG. 14, four satellites 200, 202, 204, and 206 are depicted. As shownin the next block 1308, circles are drawn around each of the satellitesso that the circles form an intersection at the center of the polar mapas shown in FIG. 14. The error input position determination can be shownas the inner section of the circles, which resemble an ellipsoid.

An important parameter in the ellipsoid representation is the ratiobetween the semi-major and semi-minor access of the ellipsoid, calledthe geometric ratio of access factor (GRAF). This factor is used alongwith the angle of the major access to compute a weighing factor, whichin effect assists the GPS system 700 to compute a more accurateposition. This is done by modifying the GPS kalman filter 802 in thevehicle GPS processing system 700 to accommodate the shape of theestimated ellipsoid and the computed northing-easting coordinates of thevehicle, as shown at a block 1310 and FIG. 14.

As shown at a block 1312, these steps are taken with respect to allpossible constellations in view of the GPS antenna 702. After allpossible satellite constellations have been computed and plotted, theoptimal satellite constellation for the desired vehicle path isdetermined as shown at a block 1312. The optimal constellation will beone that gives the least error perpendicular to the desired vehicle pathas shown at a block 1312. As shown at a block 1314, the optimalsatellite constellation data is transmitted to the vehicle 310 over thedata radio 714.

2. Differential Correction Techniques

a. Original Bias Technique

Referring now to FIG. 15, an original bias flowchart 1500 is depicted.As shown at a block 1502, the original bias flowchart computationbegins. As shown at a block 1504, the pseudorange (PSR) for eachsatellite (i) in view of the GPS antenna 702 is computed. Thepseudoranges are computed by the GPS receiver 706 located at the basestation 314. The block 1504 also shows that the positions of eachsatellite (SV (i)) where SV indicates position, and (i) indicates aparticular satellite for which this data is being computed, isdetermined.

The satellite positions are determined independently of the pseudorangecomputations made by the GPS receiver 706. The satellite positions aredetermined by decoding the GPS time data which is encoded in thenavigation signal transmitted from each satellite. This time oftransmission is the time the satellite actually transmitted thenavigation sequence which was received by the GPS receiver 706.

Because the path of the satellites is known and recorded in the almanacas described above, the position of the satellite can be determined bycorrelating the GPS transmission time with the almanac data. Thus, fortime T1 (i), the polar coordinates of the satellite (i) can bedetermined through a correlation table in the almanac.

After the steps in the block 1504 have been completed, the known basestation 314 position is subtracted from each satellite position as shownin a block 1506. Because the position of the satellites and base station314 are in polar coordinate form, a polar coordinate equation using theeuclidian norm, which is well known in the art, must be used to makethis subtraction. The actual equation subtracts the polar coordinates ofthe base station 314 from the polar coordinates of the satellite ofwhich difference the euclidian norm is computed. The square root of theeuclidian norm is then computed.

The steps shown in the block 1506 produce the actual range of thesatellite from the GPS antenna 702 located on the base station 314. Asshown in a block 1508, the pseudorange for each satellite (computed bythe GPS receiver 706 using the GPS time) and the satellite's clock biasare subtracted from the actual range determined in the block 1506. Thisstep produces the actual bias (error) of the pseudorange computationmade by the GPS receiver 706. This error is caused by many differenteffects, such as atmospheric conditions, receiver error, etc.

Because the vehicle 310 is in close proximity to the base station 314,the error in pseudorange computation is assumed to be identical.Therefore, the pseudorange error which has been determined as shown inthe blocks 1504, 1506 and 1508 can be used to modify the positionestimates produced by the vehicle 310 GPS processing system 700. Ofcourse, this error calculation could not be made at the vehicle 310 GPSprocessing system 700, because the actual position of the vehicle is notknown, whereas the actual position of the base station 314 is known.

As shown at a block 1510, the base station 314 GPS kalman filter 802 isupdated with the bias information. Finally, as shown as a block 1512,the base station 314 computed biases are transmitted to the vehicle 310using the data radios 620 and 622.

The biases are then used by the vehicle 310 GPS processing system 700 toprovide more accurate position determination by updating the vehicle 310GPS kalman filter 802.

b. Parabolic Bias Technique

As the GPS satellites rise and fall in the sky, the path formed by eachsatellite follows a parabola. Therefore, a parabolic function can becomputed which represents the path of each satellite in the sky. Turningnow to FIG. 16, part of the flowchart 1600 shown describes a method inwhich such a parabolic function (model) is computed for each satellitein the view of the base station 314 GPS antenna 702.

Referring now to FIG. 16, a block 1602 shows that the flowchart begins.As shown at a block 1604, at a time t(n), pseudoranges are determinedfor each satellite in view of the base station 314 GPS antenna 702,using the GPS receiver 706, as described above. As shown at a block1606, the pseudoranges (for each satellite) are incorporated intoparabolic best fit models for each satellite. Thus, one point on theparabolic model for each satellite is added.

As shown at a block 1608, a test is made as to whether enough points onthe parabolic model have been determined to estimate a parabolicfunction which the satellite would conform to along the rest of itspath. The number of points that have been collected will determine aparticular statistical R squared value. As shown at the block 1608, ifthis R squared value is greater than 0.98, then the parabolic model isdeemed to be accurate enough to estimate the future path of thesatellite. If the R squared value is less than or equal to 0.98, thenmore points on the parabolic model must be computed. These points arecomputed by incorporating the pseudorange data which is continuallybeing computed by the GPS receiver 706.

As shown at a block 1610, the N value increments to show that the timeat which the pseudorange is computed, as shown in the block 1604, hasincreased. Because the GPS receiver 706 outputs pseudorange data foreach satellite twice a second, each N increment should representapproximately one half second.

If enough data points have been collected such that the R squared valueis greater than 0.98, then, as shown in a block 1612, the parabolicmodel is deemed accurate enough to represent each satellite's orbitalpath. As shown in the block 1612, the parabolic model represents pointson the past and future satellite path. Now that the parabolic model iscomplete, future points on the model can be calculated as shown at a box1614. As shown at the block 1614, for the time T(n) the locus point onthe parabolic model is computed. Here the locus point is the expectedlocation of the satellite at time T(n+1). Once this locus point iscomputed, the range for the locus point (distance between the GPSantenna 702 and the satellite) is computed, as shown at a block 1616.

As shown at a block 1618, the pseudorange is then computed for timeT(n+1) (the current time). The pseudorange is computed by the GPSreceiver 706 as described elsewhere in this application. Thispseudorange is also incorporated into the parabolic best fit model, asindicated at an arrow 1626. As shown at a block 1620, the pseudorangecomputed at time T(n+1) and the base station 314 clock bias aresubtracted from the parabolic locus point range to get the bias. Thebias data are then smoothed in accordance with methods well known in theart, as shown at a block 1622. This bias is then transmitted to thevehicle 310 using the data radio 714, as shown at a block 1624.

C. Base Residuals as Bias Technique

FIG. 17 uses base residuals to compute the bias. A base residual is thedifference in the actual polar coordinate position of the base station314 and the position which is computed by the GPS processing system 700on the base station 314. To illustrate how this functions, assume thebase station 314 is at the corner of Elm and Maple streets. Also assumethe base station 314 GPS processing system 700 estimates the position ofthe base station 314 to be four miles due south of the actual knownposition of the base station (the corner of Elm and Maple). Then it isobvious that the bias is a distance equal to four miles in a due southdirection.

Because the GPS processing system 700 on the vehicle 310 is identical tothe GPS processing system 700 on the base station 314, the four mileerror in computation can be deemed to be occurring at the vehicle 310 aswell as the base station 314. The vehicle 310 can then use thisinformation in its GPS processor 710. In effect, the GPS processor onthe vehicle 310 will modify its position computations to account for afour mile due south error in the data. Thus, the position computed bythe vehicle 310 GPS processor 710 will reflect a position which is fourmiles north of the position computed by the vehicle 310 GPS processor710 which used the pseudorange computed by the vehicle 310 GPS receiver706.

Turning now to FIG. 17, a block 1704 shows that the exact polarcoordinates x0, y0, z0 of the base station must be obtained. Thepseudoranges for the satellites which are in view of the GPS antenna 702are then computed. If the GPS receiver 706 on the vehicle 310 isconfigured to read data from a particular constellation of satellites(not necessarily all which are in view), then the GPS receiver 706 atthe base station 314 must also be configured to use the sameconstellation.

The location of the base station 314 is then computed by the GPSprocessor 710 located at the base station 314. If there is a differencein this computed base station 314 location and the actual known basestation 314 location, (such as the four miles in the above example),then these differences, called residuals, as shown at a block 1710represent the new biases for the vehicle 310. As shown at a block 1712,the residuals are then outputted to the vehicle 310 over the data radio714 to be processed at the GPS processor 710.

G. Satellite Position Predictor

The present invention includes a method by which the future positions ofGPS satellites can be predicted. By thus predicting the future positionsof the satellites, the optimum satellite constellation for given siteconditions can be determined well in advance. Thus, the presentinvention will provide for the prediction of satellite availability andunavailability in a systematic manner allowing for future planning,including vehicle operation service and maintenance.

Turning now to FIG. 18, the beginning of the satellite positionpredictor flowchart is shown at a block 1802. For a particular GPSsatellite, the date and time for which the position of the satellite isdesired to be known is obtained. For example, it may be desirable toplan maintenance periods for vehicles 310 during those periods when theleast optimal constellations are available.

Once the date and time of the desired satellite position is determined,the latitude and longitude of the base station 314 must be determined asshown at a block 1806. As shown at a block 1808, the almanac data isthen obtained. The satellite position in XYZ coordinates is thendetermined from the almanac data as shown at a block 1810. Finally, asshown at a block 1812, the latitude, longitude, elevation and azimuth iscomputed from the XYZ coordinates.

H. Weighted Path History

The weighted path history technique of the present invention improvesthe positioning capability of the present invention. The weighted pathhistory technique depicted in FIG. 19, uses previous path positions toprovide a estimation of future positions. Use of this technique resultsin a reduction of position "wandering" and enhanced immunities tospurious position computations. Wandering describes the tendency of thevehicle positioning system 1000 to determine positions that deviate fromthe actual path of the vehicle 310.

Turning now to FIGS. 19 and 20, the weighted path history technique isdescribed. At a block 1902, the weighted path history flowchart begins.The position of the vehicle 310 is computed at a block 1904 by thevehicle positioning system 1000 as can be seen in FIG. 20, for vehiclepositions, 2002, 2004, 2006, 2008, and 2012 are diagrammed. The vehicle310 moves along the path 2022 which is created by the sequence of pointsjust noted.

Turning back now to FIG. 19, at a block 1906, each new positioncomputation is incorporated into the weighted path history best fitalgorithm. At a block 1908, the R square value is shown to be comparedagainst 0.98. This R squared value represents the number of positionpoints that have been taken thus far, and therefore how statisticallyaccurate a future estimation can be. The window referenced in the block08 refers to the previous number of computed position points.

If the R squared value of the new window is not greater than or equal to0.98, then a test is made at a block 1910 to see if more than 20position points have been computed. If there have been more than 20points collected, then the window is restarted as shown at a block 1914.The window is restarted in this case because, if the R squared value isless than 0.98 after more than 20 points have been collected, thecollected points are deemed to be inaccurate and are therefore notrelied upon. To restart the window means that all new data points willstart being incorporated into the best fit algorithm.

If less than or equal to 20 position points have been calculated, thenthe window is not restarted. Rather, the raw position is outputted asshown at a block 1912. This raw position is simply the position that wascomputed as is shown in the block 1904. If there were not more than 20points collected, then it is assumed that not enough points have yetbeen collected to produce a R squared value greater than or equal to0.98. After the window is restarted as shown in a block 1914 the rawposition is also output as shown in the block 1912.

If the R squared value of the new window is greater than or equal to0.98, then as shown in a block 1916, the output position is modified tobe the best fit prediction. This is depicted in FIG. 20 at a positionpoint 2010. Position point 2010 represents the position point ascomputed by the vehicle positioning system 1000. According to theweighted path history technique, the position point 2010 is differentfrom the expected position point 2006. Therefore, the actual positionoutputted is modified from the position point 2010 to become theposition point 2006. This assumes that there were enough position pointspreviously computed so that the R squared value of the new window wasgreater than or equal to 0.98 as required in the block 1908 of FIG. 19.Once the position point 2010 is modified, it is outputted to thenavigation system 1022.

I. Anti-Spoofing

It is believed that the U.S. government (the operator of the GPSsatellites) may at certain times introduce errors into the navigationdata being transmitted from the GPS satellites by changing clock andephemeris parameters. For example, during a national emergency such anaction might take place. The government would still be able to use theGPS because the government uses a distinct type of pseudo random codetransmission, called the P-code. Thus, the government could debilitatethe C/A code and carrier transmissions, causing earth receivers tocompute incorrect pseudoranges, and thus incorrect positiondeterminations. The present invention includes methods to detect andcompensate for such misleading data.

Turning now to FIG. 21, a flowchart of the anti-spoofing technique isdepicted. The beginning of the flowchart is shown at a block 2102. At ablock 2104 it is shown that the current position of satellites in viewof the GPS antenna 702 is predicted by old almanac data. Old almanacdata is data that has been previously recorded by the GPS receiver 706.The current position of the satellite is also computed using the currentephemeris data being transmitted by the GPS satellites. At a block 2106,the predicted position (using the almanac) and the computed position(using the latest ephemeris) are compared. As is shown in the block2106, the euclidian norm of the two parameters is computed and testedagainst a preset threshold. If this value is larger than the threshold,then the ephemeris data would appear to be corrupted and the latestvalid almanac data will be used instead, as is shown at a block 2108.Because the latest valid almanac data will be older than the newephemeris data, it is always the preference to use the latest ephemerisdata.

The position of the base station 314 is then computed based on the goodalmanac data and the GPS time sent by the satellites. This good almanacdata will either consist of the latest valid almanac data in the casewhere the ephemeris data is corrupted, or the ephemeris data itself whenthe ephemeris data is not corrupted. As shown at a block 2112, thecomputed base position is tested against expected values. Because thelocation of the base station 314 is known, the accuracy of thecomputation using the almanac data is readily determined. If theaccuracy is within an expected range, then the results are sent to thevehicle 310 as shown at a block 2116.

If the computed base station 314 is not within expected values, thenclock time and/or biases at the base are manipulated so that theestimated base position is as expected, as is shown at a block 2114.These results are then sent to the vehicle as shown at the block 2116.

J. Surveying

In addition to position determination and navigation of vehicles 310,the present invention can be used in a separate embodiment to accomplishsurveying in real time. Thus, the position of any point on the Earth canbe computed using the techniques of the present invention.

K. Graphic Representations

The present invention includes the production of graphic images whichallow users at the base station 314 to view the track of the variousvehicles 310 which are being navigated with the present invention. Thesegraphic images will be displayed on common video display devices and, inanother embodiment on a hard copy type terminal.

III. Navigation

A. Overview

In considering implementation of an autonomous navigation system, thereare some basic questions which any autonomous system must be able toanswer in order to successfully navigate from point A to point B. Thefirst question is "where are we (the vehicle) now?" This first questionis answered by the positioning system portion of the present invention,as discussed above in section II.

The next or second question is "where do we go and how do we get there?"This second question falls within the domain of the navigation systemportion of the present invention, discussed in this section (III).

A further (third) question, really a refinement of the second one, is"how do we actually physically move the vehicle, for example, whatactuators are involved (steering, speed, braking, and so on), to getthere?" This is in the domain of the vehicle controls subsystem of thenavigation system, also discussed below.

In the preceding and following discussions of the present invention,recall that, "system(s)" may include apparatus and/or methods.

As has been discussed implicitly above, autonomous navigation, of amining vehicle as an example, may provide certain significant advantagesover conventional navigation. Among them is an increased productivityfrom round the clock, 24 hr. operation of the vehicles. The problemspresented by dangerous work environments, or work environments wherevisibility is low, are particularly well suited to solution by anautonomous system.

There are, for instance, some mining sites where visibility is so poorthat work is not possible 200 days of the year. There are other areaswhich may be hazardous to human life because of being contaminated byindustrial or nuclear pollution. An area may be so remote or desolatethat requiring humans to work there may pose severe hardships or beimpractical. The application of the present invention could foreseeablyinclude extraterrestrial operations, for example, mining on the Moon,provided that the necessary satellites were put in Moon orbit.

In a typical application of the present invention, as shown in FIG. 3,with regard to the navigation of a mining vehicle at a mining site,there are three basic work areas: the load site, the haul segment, andthe dump site. At the load site, a hauling vehicle may be loaded withore in any number of ways, by human operated shovels for instance,controlled either directly or by remote control, or by autonomousshovels. The hauling vehicle then must traverse an area called the haulsegment which may be only a few hundred meters or may be several km's.At the end of the haul segment is the dump site, where the ore is dumpedout of the hauling vehicle to be crushed, or otherwise refined, forinstance. In the present invention, autonomous positioning andnavigation may be used to control the hauling vehicle along the haulsegment. Autonomously navigated refueling and maintenance vehicles arealso envisioned.

Referring now to FIGS. 4 and 5, Navigation of the AMT (Autonomous MiningTruck) encompasses several systems, apparatus and/or functions. The VPS(Vehicle Positioning System) 1000 subsystem of the overall AMT system asdescribed above, outputs position data that indicates where the vehicleis located, including, for example, a North and an East position.

Refering now to FIGS. 4 and 5, position data output from the VPS isreceived by a navigator 406. The navigator determines where the vehiclewants to go (from route data) and how to get there, and in turn outputsdata composed of steer and speed commands to a vehicle controlsfunctional block 408 to move the vehicle.

The vehicle controls block then outputs low level commands to thevarious vehicle systems, 310, such as the governor, brakes andtransmission. As the vehicle is moving towards its destination, thevehicle controls block and the VPS receive feed-back information fromthe vehicle indicative of, for example, any fault conditions in thevehicle's systems, current speed, and so on.

Navigation also must include an obstacle handling (detection andavoidance) capability to deal with the unexpected. A scanning system 409detects obstacles in the vehicle's projected trajectory, as well asobstacles which may be approaching from the sides and informs thenavigator of these.

The navigator may be required to then decide if action is required toavoid the obstacle. If action is required, the navigator decides how toavoid the obstacle. And after avoiding the obstacle, the navigatordecides how to get the vehicle back onto a path towards its destination.

Referring now to FIG. 35, titled the context diagram, and FIG. 37A-37Ddefinitions of the communications, which are shown as circles withnumbers in them, are provided below:

502. Host commands & queries:

Commands given by the host to the vehicle manager. These commands couldbe of several types:

initiate/terminate;

supply parameters;

emergency actions; and

directives.

Queries inquire about the status of various parts of the navigator.

504. replies to host:

These are responses to the queries made by the host.

432. position data:

This is streamed information provided by the VPS system.

416. Range data:

This is range data from the line laser scanner.

432. VPS control:

These are commands given to the VPS system to bring it up, shut it downand switch between modes.

416. scanner control:

These are commands sent to the laser scanner to initiate motion and setfollow velocity profile.

420. steering & speed commands

These are commands given to the vehicle to control steering and speed.These commands are issued at the rate of 2-5 Hz.

Referring to FIG. 5, in a preferred embodiment of the present invention,as described above, both the VPS and the navigator are located on thevehicle and communicate with the base station 314 to receive high levelGPS position information and directives from a host processing system,discussed below. The system gathers GPS position information from thesatellites 200-206 at the base station and on-board the vehicle so thatcommon-mode error can be removed and positioning accuracy enhanced.

In an alternate embodiment of the present invention, portions of the VPSand navigator may be located at the base station.

The host at the base station may tell the navigator to go from point Ato point B, for instance, and may indicate one of a set of fixed routesto use. The host also handles other typical dispatching and schedulingactivities, such as coordinating vehicles and equipment to maximizeefficiency, avoid collisions, schedule maintenance, detect errorconditions, and the like. The host also has an operations interface fora human manager.

It was found to be desirable to locate the host at the base station andthe navigator on the vehicle to avoid a communications bottleneck, and aresultant degradation in performance and responsiveness. Since the hostsends relatively high-level commands and simplified data to thenavigator, it requires relatively little communication bandwidth.However, in situations where broad-band communication is available tothe present invention, this may not be a factor.

Another factor in determining the particular location of elements of thesystem of the present invention, is the time-criticality of autonomousnavigation. The navigation system must continually check its absoluteand relative locations to avoid unacceptable inaccuracies in following aroute. The required frequency of checking location increases with thespeed of the vehicle, and communication speed may become a limitingfactor even at a relatively moderate vehicle speed.

However, in applications where maximum vehicle speed is not a primaryconsideration and/or a high degree of route following accuracy is notcritical, this communication factor may not be important. For example,in rapidly crossing large expanses of open, flat land, in a relativelystraight path, it may not be necessary to check position as often in thejourney as it would be in navigating a journey along a curvaceousmountain road.

Conceptually, the navigation aspects of the present invention can bearbitrarily divided into the following major functions:

route planning/path generation;

path tracking; and

obstacle handling.

The function of the present invention are discussed below.

B. Route Planning/Path Generation

1. Introduction

Autonomous vehicle navigation in accordance with the present invention,conceptually consists of two sub problems, path generation and pathtracking, which are solved separately.

Path generation uses intermediate goals from a high level planner togenerate a detailed path for the vehicle 310 to follow. There is adistinct trade-off between simplicity of representation of such plansand the ease with which they can be executed. For example, a simplescheme is to decompose a path into straight lines and circular curves.However, such paths cannot be tracked precisely simply because ofdiscontinuities in curvature at transition points of segments thatrequire instantaneous accelerations.

Following path generation, path tracking takes, as input, the detailedpath generated and controls the vehicle 310 to follow the path asprecisely as possible. It is not enough to simply follow a pre-made listof steering commands because failure to achieve the required steeringmotions exactly, results in steady state offset errors. The errorsaccumulate in the long run. Global position feedback 432 may be used tocompensate for less than ideal actuators. Methods have been developedfor the present invention which deviate from traditional vehicle controlschemes in which a time history of position (a trajectory) is implicitin the plan specified to the vehicle 310.

These methods are appropriately labeled "path" tracking in that thesteering motion is time decoupled; that is, steering motions aredirectly related to the geometric nature of the specified path, makingspeed of the vehicle 310 an independent parameter.

Referring now to FIG. 3, an autonomous vehicle 310 may be required toleave a load site 318, traverse a haul segment 320 to a dump site 322,and after dumping its load, traverse another haul segment to a serviceshop 324, under the direction of the host 402. The host 402 determinesthe vehicles' destinations, which is called "cycle planning." Once thework goals have been determined by "cycle planning", determination ofwhich routes to take to get to a desired destination must beaccomplished by "route planning."

"Route planning" is the determination of which path segments to take toget to a desired destination. In general, a route can be thought of as ahigh-level abstraction or representation of a set of points between twodefined locations. Just as one can say to a human driver "take route 95south from Lobster, Me. to Miami, Fla.," and the driver will translatethe instruction into a series of operations (which may include startingthe vehicle 310, releasing the brake 4406, engaging the transmission4610, accelerating to the posted speed limit, turning the steering wheel4910, avoiding obstacles 4002, and so on), the autonomous navigationsystem of the present invention performs similarly. As used in thesystem of the present invention, a "route" is a sequence of contiguous"segments" between the start and end of a trip.

An autonomous vehicle 310 may begin at any position in the sequence andtraverse the route in either direction. A "segment" is the "path"between "nodes." A "node" is a "posture" on a path which requires adecision. Examples of nodes are load sites 3318, dump sites 322, andintersections 326.

There are various types of segments. For instance, there are linear andcircular segments. Linear segments (lines) are defined by two nodes.Circular segments (arcs) are defined by three nodes.

"Postures" are used to model parts of a route, paths and nodes forinstance. Postures may consist of position, heading, curvature, maximumvelocity, and other information for a given point on the path.

A "path" is a sequence of contiguous postures.

A segment is, therefore, a sequence of contiguous postures betweennodes. All segments have a speed associated with them, which specifiesthe maximum speed with which the vehicle 310 is to traverse thatsegment. The navigator 406 can command slower speeds, if necessary, tomeet other requirements.

Determining which postures are required to define a path segment byanalytical, experimental or a combination of both, is called "pathplanning" in accordance with the present invention. To bring thediscussion full circle, a sequence of contiguous routes, as mentionedabove, is referred to as a "cycle," and a vehicle's 310 work goalsdetermine its "cycle."

Therefore, to define a route one must first define the nodes andsegments. Next, the nodes and segments must be ordered. Finally theroutes must be defined by specifying where in the ordered set a route isto begin, and in which direction the ordered set is to be traversed (SeeFIG. 22 which illustrates these concepts of the present invention).

The aforementioned method of defining routes was developed for memoryefficiency in the present invention. It is also a convenient way todefine many routes on a specific set of nodes and segments.

In a real world example of the present invention, picture a site wherethere are many intersecting roads 326. A route programmer would definenodes at the intersections, and segments to define the roads between theintersections. Routes would therefore be determined by the roads andintersections. There will however, be many ways to get from point A topoint B (many routes) with a fixed set of intersections and roads.

The path-tracking method of the present invention (discussed below) usesroute curvature to steer the vehicle. Methods of route definition usinglines and arcs do not provide for continuous curvature. Clothoid curvesare another way to define routes.

Another method of defining routes developed by the inventors, fitsB-splines to the driven data. B-splines provide continuous curvature andtherefore enhance tracking performances. In addition, since B-splinesare free form curves, a route may be defined by a single B-spline curve.By using free form curves, a more robust method (semi-automatic) forfitting routes to data collected by driving the vehicle over the routesis produced by the presnent invention.

Referring to FIGS. 4 and 22, in operation, the host 402 from the basestation 314 commands an identified vehicle 310 to take route N from itspresent location. The navigator 406 functions to generate a path bytranslating "route 1" into a series of segments, each of which may havea "posted" or associated maximum speed limit, which together form agenerated path for the vehicle to attempt to follow. By specifyingroutes and commanding the autonomous vehicle 310 with high-levelcommands this way, enormous data requirements and inefficiencies are inthe present invention avoided in giving directions.

The navigator 406 stores the routes as a linked-list of path segments,rather than the set or series of sets of individual points. Thesesegments are also abstractions of the set of points between definedlocations or nodes.

A LINKER then takes given path segments and generates a linked-list ofcontrol points, allowing for flexibility and efficiency. Path segmentsare shared by different routes, as is shown in FIG. 22.

The path segments are stored in a memory called the TARGA 5302 as a setof arcs, lines, and postures, for instance, in one embodiment of thepresent invention. An analytical generator function generates pathsusing these arcs, lines and postures. In another embodiment of thepresent invention, B-splines are used as a mathematical representationof a route, as mentioned above.

In another embodiment or the present invention, "clothoid" curves areused in generating path segments. These are discussed below.

a. Clothoid Path Segments

As discussed above, part of the navigation problem addressed and solvedby the present invention is really two sub-problems: path planning andpath generation. These are solved separately by the present invention.

Path planning proceeds from a set of sub-goals using some pathoptimization function and generates an ordered sequence of "objective"points that the vehicle 310 must attain.

The challenge of path generation is to produce from the objective points(of path planning), a continuous, collision-free path 3312, smoothenough to be followed easily by the autonomous vehicle 310. For example,a simple scheme is to decompose a path 3312 into straight lines andcircular curves. The path 3312 is then converted into a sequence ofexplicit directives provided to the vehicle 310 actuators to keep thevehicle on the desired path 3312. It should be noted that there is adistinct trade-off between simplicity of representation of such plansand the ease with which they can be executed.

The ability of an autonomous vehicle 310 to track a specified path 3312is dependant on the characteristics of the path. Continuity of curvatureand the rate of change of curvature (sharpness) of the generated path3312 are of particular importance since these parameters dictatesteering motions required of a vehicle 310 for it to stay on the desiredpath 3312. Discontinuities in curvature are impossible to follow sincethey require an infinite acceleration. For some autonomous vehicleconfigurations, the extent to which the sharpness of a path is linear isthe extent to which steering motions are likely to keep the vehicle onthe desired path 3312, since linear sharpness of a path equates toapproximately constant velocity of steering.

One method used by the present invention, is to compose paths as asequence of straight lines and circular arcs. This method suffers fromdiscontinuities in curvature where arcs meet. Another method of thepresent invention, is to use polynomial splines to fit paths betweenobjective points. Splines assure continuity in curvature, but do notmake any guarantees of linearity in sharpness.

Inability to track the requisite curvature results in steady stateoffset errors from the desired path 3312. These errors can becompensated for by closing a feedback loop on position 3314. This issufficient in those scenarios where the response of the actuators isfast enough to guarantee negligible tracking errors and position sensingis accurate, such as on a factory floor. However, path tracking issimpler if the path is intrinsically easier to track.

The method of the present invention, generates explicit paths that passthrough a sequence of objective points. A derivative method of thepresent invention, replans parts of the path dynamically in case thetracking error becomes large or the desired path is changed.

b. Modeling A Vehicle Path

Any path can be parameterized as a function of path length (s) byposition coordinates (x(s), y(s)) 3304. That is, position coordinates xand y can be written as explicit functions of the path length s. Heading(0(s)) 3318 and curvature (c(s)) 3316 can be derived: ##EQU1##

The quadruple of these parameters, p=(x,y,0,c), is a posture 3314 thatdescribes the state of an autonomous vehicle 310 at any point in time.

c. Clothoid Curves

Clothoid curves are used in an embodiment of the present invention. Theyare a family of curves that are posture-continuous, and are distinct inthat their curvature varies linearly with the length of the curve:

    c(s)=ks+C.sub.i                                            (EQ. 3)

where k is the rate of change of curvature (sharpness) of the curve andsubscript i denotes the initial state. A clothoid curve segment 2002 isshown in FIG. 26.

Given a n initial posture, sharpness of the clothoid segment and thedistance along that segment, position, orientation and curvature at anypoint are calculated as follows: ##EQU2##

d. Generation of a Posture-Continuous Path

Practical navigation problems require composite paths whose range andcomplexity cannot be satisfied by a single clothoid segment. Most pathsrequire multiple segments that pass through a sequence of objectivepoints.

(1) Existing Methods

Hongo et al., "An Automatic Guidance System of a Self-ControlledVehicle--The Command System and Control Algorithm", Proceedings IECON,1985, MIT Press, 1985. proposed a method to generate continuous pathscomposed of connected straight lines and circular arcs from a sequenceof objective points. While paths comprised solely of arcs and straightlines are easy to compute, such a scheme leaves discontinuities at thetransitions of the segments as discussed above.

Kanayama et al., "Trajectory Generation for Mobile Robots", RoboticsResearch: The Third International Symposium, ISIR, Gouvieux, France,1986 makes use of paired clothoid curves with straight line transitionsbetween postures. The constraint of straight line transitions is due tothe integrals in Eqs. (7) and (8) which do not have closed formsolutions. Kanayama simplified this problem by requiring c_(i) =0. Also,by rotating the reference frame by the amount of the initialorientation, 0_(i) =0; only a straight forward approximation of ##EQU3##is left.

Kanayama's method leads to paths that are sharper at some points andless compact than necessary, with adverse consequences to control. Inaddition, the requirement for straight-line transitions precludes thelocal replanning of paths because there are no guarantees that a segmentto be replanned will include an uncurved section.

2. Path generation from a sequence of points

A two-step method of the present invention, to generate a uniqueposture-continuous path from a sequence of points is now described.

Referring now to FIGS. 23, 24 and 25), the first step is to derive asequence of unique postures 2302, 2304, 2306, 2308, 2310 from theobjective points. The second step is to interpolate between thosepostures with clothoid segments. Heading and curvature at the startingand ending positions 2402, 2404 are presumed. Let P_(i), P_(f) be thestarting and ending postures 2402, 2404, respectively.

It is not always possible to connect two postures with one clothoidcurve segment because four equations EQ.2, EQ.4, EQ.5, and EQ.6 cannotbe satisfied simultaneously with only two parameters (sharpness k andlength s) of a clothoid curve.

In order to satisfy the four equations EQ.2, EQ.4, EQ.5, and EQ.6 oneneeds at least two clothoid curve segments. However, the general problemcannot be solved with two clothoid segments because if k_(i) and k_(f)have the same sign, in most cases, a third segment is required inbetween. One adequate set of the clothoids connecting a pair ofneighboring associated postures is the set of three clothoid segments(k,s_(i)), (-k,s₂), (k,S₃). The subscripts denote the order of theclothoid segments from P_(i). This combination is plausible for thefollowing reasons:

1. The signs of k for the first and the last clothoid segments are thesame.

2. k for the second clothoid segment is equal in magnitude and oppositein sign to that of the first and last segments. This enables the curveof three clothoid segments to satisfy the curvature variation betweenthe starting and the ending curvatures by varying s₁, s₂, s₃, eventhough the sign of the first and the last clothoid segments satisfiesthe curve location requirement.

3. There are four variables in the combination: k, s₁, s₂, s₃. It ispossible to find a unique solution satisfying the following fourequations which describe the mathematical relationship between thestarting and the ending postures. ##EQU4##

Referring now to the method shown in FIG. 27. Since equations 9 and 10above contain Fresnel integrals, for which there is no closed formsolution, the values of k,s1,s2,and s3 are computed.

Paths resulting from the method have the following advantages over othermethods:

The method proceeds from an arbitrary sequence of points. Generation ofpostures is essential to exploratory planning where goals are commonlyposed as an evolving string of points. Paths generated by the methodpass through all the objective points whereas paths from Kanayama'smethod and the arc method are only proximate to many of the pointsbecause these methods start from a sequence of postures.

The method guarantees continuity of position, heading and curvaturealong the path. Further, sharpness is piecewise constant.

Paths generated by the method always sweep outside the acute anglesformed by straight line connection of the way points. The resultingpaths are especially useful for interpolating around obstacles that arecommonly on the inside of angles. In contrast, Kanayama's paths arealways inside the angles.

3. Clothoid Replanning

Paths Clothoid replanning is done either to aquire the path initially,or to guide the vehicle 310 back to the desired path 3312 through normalnavigation according to the present invention.

To avoid abrupt accelerations in an attempt to make gross corrections intracking a pre-specified path, a path replanner is used by the presentinvention to generate a new path which converges smoothly to the desiredpath 3312 from the current position. Replanning decomposes to two subproblems:

1. Determining the point of convergence to the intended path 3308.

2. Planning a path from the current position 3302 to the convergentpoint 3308.

Reference is made to FIG. 28, which graphically illustrates replanning apath in accordance with the present invention. A pre-specified pathconsists of interpolations 2804 between postures (k,s)_(m) (m=1, . . . ,n) 2804-2810 and the postures P_(m) (located at the end of segment(k,s)_(m)). Assuming that the vehicle 310 deviates from the path betweenP_(m) and P_(m) +1, then P_(m) +2 is chosen as the posture 334 to whichthe replanned path 2816 converges. The distance to P_(m) +2 is variable.

A curve composed of two curve segments is fitted to the postures (thecurrent posture and the one chosen as a convergence posture) to obtain areplanned path 2816, satisfying four governing posture equations EQ.7,EQ.8, EQ.9, EQ.10. If we assume that the threshold that determineswhether a path is to be replanned or not is much smaller than the lengthof each clothoid curve segment (k,s)_(m), we can find a newposture-continuous path ((k*_(k) +1, s_(k) +1), (K*_(k) +2, s*_(k) +2))using a small perturbation from known ((k_(k) +1, s_(k) +1), (k_(k) +2,s_(k) +2)). Since the replanned path 2816 is not likely to be very farfrom the original path 3312, two clothoid segments can be used.

4. Summary

In accordance with the present invention, generation of continuous pathsfor autonomous vehicles 310 can use clothoid segments to generate pathsnot only because the resulting path is posture continuous but alsobecause linear curvature along the curve leads to steering angles thatvary approximately linearly along the path, facilitating path tracking.

The approach of the present invention is as follows: first, a sequenceof the postures is obtained using the objective points. Then, each ofthe adjacent postures is connected with three clothoid curve segments.

The present method accrues additional advantages in that preprocessingof the objective points is not necessary as with arcs and zero curvatureclothoids. Further, the geometry of the paths generated always sweepsoutside the acute angles formed by straight line connection of the waypoints. These are especially useful for interpolating around obstaclesthat are commonly on the inside of angles.

From the set of stored arcs, lines and postures, clothoid curves,B-splines, and so on, points along a path are generated with the VPSposture block.

Advantages of the present invention's handling routes in this way,besides reducing the bandwidth requirements between the host and thevehicle, effects data compression reducing data storage requirements,and functions to smooth-out paths.

5. B-Splines

B-splines are well known by mathematicians and those familiar withcomputer graphics (see "Mathematical Elements for Computer Graphics," byDavid F. Rogers and J. Alan Adams, McGraw-Hill Book Company, New York,N.Y., pages 144 to 155) as a means of describing the shape of a seriesof points by a specifying the coefficients of a polynomial equation.This curve fit function is an Nth order polynomial, where N is userspecified and depends on the desired shape of the curve. The B-splinecurve can be of any order and are continuous to the order of the curvefit function minus one.

B-splines are used in an embodiment of the present invention. B-splineslend themselves well to path generation in the present invention becausean arbitrarily long path can be described by a low number ofcoefficients, thus reducing the amount of data storage. Provided thatthe order of the curve fit function is high enough (three or larger),then the generated path will be smooth in curvature, resulting in a pathwhich is inherently easy to track with the aforementioned embodiments ofthe present invention.

FIGS. 29 shows an example of B-spline curves.

2. Route Creation and Storage

a. Introduction:

In one embodiment of the present invention, in order to create routesfor a site 300, data is first collected from the VPS system 1000 andstored while a human drives the vehicle 310 over the road system of thework site 300. Nodes and segments are then fitted to the stored drivendata, and organized into routes per the aforementioned procedure.

An application on an APOLLO COMPUTER (now HEWLETT-PACKARD Co. of PaloAlto, Calif.) work station (a graphics display system, not shown) wasdeveloped to graphically fit route data to the stored driven data and tofurther define routes (that is, speeds, sequences, starting point,traversal direction). Any graphics work stations equivalent to theAPOLLO could be used.

Once the routes for a site are defined, the route data is written to apermanent storage device. In one embodiment of the present invention,the storage device used is a bubble memory cartridge 5302 with anassociated reader/writer. The bubble memory device 5302 is durable andretains the data when power is disconnected. The APOLLO application iscapable of writing data to a cartridge 5302 and reading data from acartridge 5302.

As implied above, routes in the present invention may be predefined, orthey may be generated dynamically.

In mining applications, generally a site 300 is surveyed and roads arepre-planned, carefully laid out and built. The routes used by thenavigation system may then either be obtained from a manually createdcomputer data base (created specifically to be used by the navigationsystem), or alternately, a vehicle may be physically driven over theactual routes on site to learn the routes as described above. In thelearning method, several trips over a given route may be made. Then thevariations in the data (due for instance to driver weaving) areaveraged, and a smoothed-out best fit developed.

b. Route Definition:

In one embodiment of the present invention, the following method is usedfor route definition.

1. Define the nodes and segments upon which the routes will be built.Place the node and segment data into an array called the "routeData"array. Each record in the array contains the following information:

1. Type of item (that is, node, linear segment, circular segment, end ofroute marker).

2. If node item, define the north and east coordinates of the node;

else if linear segment item, define the speed along the segment;

else if circular segment item, define the north and east coordinates ofthe center, the radius, the direction the circle is traversed (that is,clockwise, or counterclockwise), and the speed along the segment;

else if end of route marker, there is no other information.

2. Link the node and segment data together into sequences. The sequencesare simply an array of indexes into the routeData array. Each sequencemust begin with an end of route marker, followed by a node, then theremainder of the sequence alternate between segments and nodes until thesequence is terminated by another end of route marker. An examplesequence would be,

1, 6, 3, 4, 7, 9, 10, 23, 78, 1 where the integers are indexes into therouteData array.

3. Finally define a route by specifying an index into the sequence arrayand whether to index through the sequence in the positive or negativedirection. Place the index and index direction into an array called the"routeSpec" array. An item in the route spec array may look like thefollowing:

6, 1 This specification defines a route which begins at node 6 and isindexed in the positive direction.

78, -1 This specification defines a route which begins at node 78 and isindexed in the negative direction.

A user simply tells the vehicle which item in the routeSpec array to useas a route.

4. The aforementioned data is stored onto the storage device in theorder which it was defined in steps 1-3.

c. Navigator Route Usage:

The following describes how the navigator 406 uses the defined routesfrom the above method of the present invention.

When the navigator 406 is powered on it reads the route information fromthe storage device 5302 and stores it in RAM in the syntax alreadypresented.

Next the operator specifies a route for the vehicle 310 to follow.Again, the route is simply an index into the routeSpec array.

When the navigator 406 decides that all systems are ready forauto-operation, it sends a message to the vps₋₋ posture task 5324telling it to engage.

The vps₋₋ posture task 5324 then determines the position, along theroute which is closest to the vehicle's 310 present position 2812. Thesearch for the closest position 284 on the route proceeds as follows:

1. A pointer is set to the first segment in the route.

2. The perpendicular distance from the vehicle position to the segmentis determined.

3. The pointer is moved to the next segment in the route.

4. The perpendicular distance from the vehicle position to the nextsegment is determined.

5. Repeat steps 3 and 4 until the end of route marker 2218 is reached.

6. Determine the distance from the vehicle position to the end points2218 of the route.

7. Set a pointer to the route segment which had the closest distance andstore the coordinates of the closest distance.

The vps₋₋ posture task 5324 then uses the description of the route(lines, arcs and speeds) to generate posture at one meter intervals. Thetask 5324 generates a predefined distance of postures plus a safetymargin and puts the postures into a buffer 3000. To generate a posturewhich is one meter from a given posture the vps₋₋ posture task 5324 usesthe following procedure:

1. Determine the type of segment from which the given posture wasgenerated.

2. Use the proper formula for the type of segment to determine thechange in north and east per meter of segment length.

3. Add the change in north and east per meter to the last given posture.

4. If the generated posture is beyond the end of the current segment,set a pointer to the next segment and repeat steps 2 and 3. else, returnthe generated posture.

The vps₋₋ posture task 5324 then informs the executive 5316 that it isready for tracking.

As the autonomous vehicle 310 moves along the posture in the buffer3000, the safety margin 3006 is depleted. When the safety margin isbelow a specified amount, the vps₋₋ posture task 5324 generates anothersafety margin 3006 of postures and appends them to the current buffer3000. The vps₋₋ posture task 5324 depletes the posture buffer 3000 bymonitoring the current position 2812 of the vehicle 310 and moving apointer 3002 in the buffer 3000 to the nearest posture. The posturebuffer 3000 is constructed as a ring which is traversed in the clockwisedirection (see FIG. 30, Posture Ring Buffer). That is, postures areplaced in the ring such that the direction of vehicle travel correspondsto a clockwise traversal of the posture ring buffer 3000. Therefore, asthe vehicle 310 moves the pointer 3002 to the nearest posture in thebuffer 3000 will be moved in the clockwise direction. When the pointer3002 will be moved in the clockwise direction, memory in the ring behindposture (counterclockwise of the pointer) is free to be over written.

Step 7 (in the search routine above) is registered until the end ofroute marker 2218 is reset at which time the vps₋₋ posture task 5324ceases to generate posture and informs the executive 5316 that it hasreached the end of the route.

As mentioned above, a path is as a series or sequence of contiguous"postures." A posture includes the speed and steering angle required tobe on track. A posture may include latitude, longitude, heading,curvature (1/turning radius), maximum velocity and distance to nextposture information.

3. Posture Generation

The tracking method of the present invention, requires certaininformation about the route it is tracking. The information is containedin a packet called a "posture" 3314. A single posture 3314 may containposition (that is, north and east coordinates), heading, and curvaturedata, for a specified location on the route. Therefore, a way ofproducing posture data from the route specification is required inaccordance with the present invention.

Among the navigator tasks, (discussed below) is a task which reads theroute information and produces postures at intervals (one meter forinstance) along the route which are used by the tracking method. In oneembodiment of the present invention, each posture requires 36 bytes ofmemory which translates to about 36 k of memory for each kilometer ofroute. To reduce the memory requirements, the navigator buffers posturedata.

The task which produces the postures reads the current position of thevehicle 310, finds the nearest point on the route to the currentposition, then generates a specified number of postures ahead of thevehicle 310. The number of postures generated is dependent on themaximum stopping distance of the vehicle 310. That is, there shouldalways be enough postures in the buffer 3000 to guide the vehicle 310 toa stopping point.

In the B-spline approach to route definition according to the presentinvention however, the need for a posture buffer is eliminated, sincethe tracking method is able to directly produce posture information fromthe B-spline curve.

C. Path Tracking

1. Introduction

Path tracking or following is a critical aspect of vehicle navigationaccording to the present invention. The technique of the presentinvention uses position based navigation (rather than vision basednavigation used in conventional navigation systems) to ensure that thecorrect autonomous vehicle path 3312 is followed. The present inventionis also innovative in that it provides for separate control of steeringangle 3116 and vehicle speed 3118. FIG. 36 graphically illustrates thepath tracking system 3100 of the present invention.

For an autonomous vehicle 310 according to the present invention totrack specified paths, it is necessary to generate referenced inputs forthe vehicle servo-controllers. Thus, path tracking can be considered asa problem of obtaining a referenced steering angle and a reference speedfor the next time interval in order to get back to the referenced pathahead from the current deviated position.

In general terms, path tracking is determining the autonomous vehiclecommands (speed, steer angle) required to follow a given path. Given apre-specified steering angle, driven wheel velocity values and errorcomponents, the command steering and driving inputs are computed in thepresent invention.

2. Considerations

a. Global position feedback:

The path to be tracked is specified in Cartesian coordinates. If thecontrol scheme consists of only a servo-control to reference steeringcommands, vehicle position and heading errors accumulate. Position andheading result from integrating the whole history of steering anddriving. Thus, it is necessary to feedback vehicle position 3304 andheading 3318 in Cartesian space.

Consequently, referenced inputs to the servo-controllers are generatedin real time, based on positioned feedback 3114 (as shown in FIG. 36).

b. Separate steering and driving control:

Steering and driving reference inputs are computed in the presentinvention, from the given path and vehicle speed, respectively. Thisenables easy integration of path tracking with other modules of thepresent invention, such as collision avoidance.

3. Embodiments

a. Tracking Control Structure

One of the challenges of vehicle autonomy is to determine the steeringinputs required to track a specified path. For conventionally steeredvehicles, in the present invention the desired path and the desiredspeed along the path can be tracked separately reducing the problem toone of controlling the steering. (A path, for this discussion, being ageometric curve independent of time in contrast to a trajectory, whichis a time history of positions.) Steering angles are planned from thedesired path 3312 and sensed vehicle positions. These angles arecommanded to the vehicle via a steering controller 3104.

The functional block diagram in FIG. 31, shows a tracking controlstructure according to the present invention.

In kinematic steering schemes, errors in position, heading and curvatureare reduced based on the geometry of the errors without consideration ofactuator saturation, compliance, any friction or mass terms. Tuningvalues, such as look-ahead distance and selection of curvature on thepath, are selected through empirical trials and simulations in order toget good performance.

In a manually driven vehicle, the look-ahead distance is the distance3310 in front of a vehicle that a driver looks during driving. Thelook-ahead distance in the present invention, is the distance by whichthe errors in position, heading and curvature are planned to be reducedto zero. It varies with the speed of the conventional or autonomousvehicle.

Varying the look-ahead distance varies the degree to which steeringadjustments must be made to effect a change of course. Look-aheaddistance is discussed in more detail in a following section.

However, real vehicles depart from kinematic idealization, and theircontrol response departs accordingly. As vehicle speed, mass and pathconditions change, actual vehicle response departs even further fromkinematic idealization. Hence, kinematic idealization is generally validonly at low speeds with constant conditions.

An embodiment of the present invention uses a model which includesconsiderations of cornering stiffness, mass and slip angle. The controlproblem is formulated as a linear quadratic optimal tracking problemwhere the errors in position, heading and curvature are minimized basedon the vehicle control model.

The optimal path and controls are computed from the desired path 3312and the currently sensed vehicle position using the current errors asinitial conditions to the optimal control problem. A few computedsteering angles along the initial part of the optimal path are used asreferences to the low level steering controller for the next sensingtime interval.

This preview optimal steering planning has the advantage of guaranteeingstability and optimality with respect to the given performance index.The optimal preview control method of the present invention, is centralto the steering planning of an autonomous vehicle.

Turning again to FIG. 31, the inner loop 3116 of steering control 3104is executed on the order of 10 milliseconds, while the outer loop 3114is closed at the rate of 0.25-0.5 second.

The following procedure is used to close the loop on position. Aftersensing the current position (Pa.sub.,k) 3210, the posture at the end ofthe current time interval (P_(a),k+1) 3216 is expected.

Then, the desired posture at the end of the next time interval(P_(d),k+2) 3218 is computed in a referenced steering angle between(P_(a),k,+1) 3216 and (P_(d),k+2) 3218 are determined.

Significantly, as mentioned above, these vehicle and path techniques ofthe present invention, decouple steering control from velocity controlat the vehicle.

b. Quintic method:

Shown in the navigator task diagram, FIG. 53, which is discussed in moredetail below, is a functional block called the tracker 5306. The tracker5306 operates to construct a smooth path back to the desired or correctpath. In one embodiment of the present invention, as mentioned above, aquintic method is used. This involves a fifth order curve in error spacefor steering commands.

The quintic polynomial method of the present invention, replans asimple, continuous path that converges to a desired path in somelook-ahead distance 3310 and computes a steering angle corresponding tothe part of the replanned path 2816 to be followed for the next timeinterval.

If the desired path is considered as a continuous function of positionand the vehicle is currently at Pa 3320, an error vector can becalculated (FIG. 33) that represents error in the distance transverse tothe path (e0) 3322 relative to Po 3304, in heading (Bo) 3322, and incurvature (yo) 3404. If the vehicle is to be brought back onto thespecified path within distance L 3310 (measured along the referencepath), six boundary conditions can be stated corresponding to theinitial errors and to zero errors at PL. ##EQU5##

A quintic polynomial can be constructed to describe the replanned path(in error space) as follows:

    ε(s)=α.sub.0 +α.sub.1 s+α.sub.2 s.sup.2 +α.sub.3 s.sup.3 +α.sub.4 s.sup.4 +α.sub.5 s.sup.5(EQ. 12)

where sε 0, L!

The expression for e(s) gives the error along the replanned path 2816from Po 3304 to PL 3308. The second derivative describes path curvature,which can in turn, be used to calculate a steering command to guide thevehicle back to the desired path 3312. Variation in the steering angle3116 from the replanned path 2816 (or in error space) is computed fromthe second derivative of error function e(s). Then, curvature along thenew path can be computed as: ##EQU6##

The reference steering angle 3112 along the new path can be convertedfrom curvature. Since this procedure is executed at every planninginterval, the entire new path back to the reference path 3312 is notrequired. Only the steering angle 3112 for the next time interval iscomputed from the curvature at the point on the new path that can beachieved in the next time interval.

The look-ahead distance, L 3310, is a parameter that can be used toadjust how rapidly the vehicle steers to converge to the desired path.Additionally, better performance is obtained if L 3310 is chosenproportional to the vehicle speed because for small values of L 3310,the vehicle oscillates around the path 3312, while for large values of L3310 the variation introduced by the quintic polynomial is small enoughthat the tracking performance is poor.

Since there are six boundary conditions used: e_(p0) --error in positionat the current position (distance) and e_(p1) , (look-ahead), e_(h0)--error in heading and e_(h1) (look-ahead), and e_(c0) --error incurvature and e_(c1) (look-ahead), a fifth order curve is required. Thisis used to generate a steering angle 3112.

Recall that path tracking schemes in general, perform better when thepath specified is intrinsically easier to track. This is especially thecase when the steering actuators are slow compared to the speed of thevehicle.

Other vehicle characteristics, like steering response, steeringbacklash, vehicle speed, sampling, and planning time intervalssignificantly affect vehicle performance. As expected, at higher vehiclespeeds, faster and more accurate actuators are necessary, if sensing andplanning time intervals are kept constant.

An advantage of the quintic polynomial method in general, is that it issimple, and reference steering angles can be computed very easily.However, since there is no consideration of vehicle characteristics(mass, inertia, time delays, vehicle ground interaction, and so on) inthe control scheme, stability and convergence are not guaranteed.

The parameter L 3310 (look-ahead distance) can be adjusted to modifyresponse to the vehicle, and the value of L 3310 can be chosen based ontrial and error. This scheme has provided good results at speeds up toapproximately 28 Km per hour at the time this application disclosure wasprepared.

The method used by the tracker of the present invention is:

(1) estimate the next position by either averaging or evaluating thestates of position;

(2) compensate for delays using either of the estimating methods

(3) dynamic look-ahead changes at different speeds--the coefficients ofthe quintic: look-ahead distance.

c. Latency and slow system response

An additional path tracking embodiment of the present invention, usesvarious compensation techniques to improve vehicle responsecharacteristics. This is used in conjunction with the quintic polynomialmethod to realize improved tracking performance.

Some vehicle response characteristics include latency of vehicle controlcommands, slow system response, and vehicle dynamic characteristicsincluding vehicle-ground interaction (VGI), (slip angle and under/oversteer).

The latency of vehicle commands was compensated in one embodiment of thepresent invention, by modifying the vehicle control hardware to reducetime delays, and by utilizing a method which sets control commands farenough in advance to compensate for the existing delays.

Decreasing the time lag between when the vehicle position is sensed andwhen the command is issued, reduces prediction errors, which is requiredto plan steering angles, and results in better tracking performance.

A varying look-ahead distance with speed also improves the trackingperformance in comparison to the constant look-ahead distance.

A tracking method outputs steering and speed commands over a serial linkto a vehicle control system. The vehicle control system is amulti-processor, multi-tasking system, relying on a mailbox queue forcommunication between tasks.

This mailbox queue is composed of two types of queues, a highperformance queue and an overflow queue. During high data flow ratesfrom the tracking task, the high performance queues spill into theoverflow queue, degrading the performance of inter-task communication.This can result in total latency times between the tracking task andactual steering actuator commands which are on the order of seconds.

The steering dynamics may be modeled as a first order lag system. Ittakes a period equivalent to one time constant for a first order lagresponse to reach approximately 63% of the desired final value. As canbe appreciated, for slow systems with large time constants, the responsetime can be significant.

To resolve the latency and response problems, hardware may be adjustedto be used in close conjunction with the tracking method to controlvehicle steering, and a new control scheme devised to compensate forpure time delay and poor response.

The hardware may be adjusted, for example, to reside on the same backplane as the processor which executes the tracking method and controlsthe vehicle steering system directly. This serves to eliminate delaysdue to the serial link and queuing.

To compensate for the remaining delays (delays due to processing time ofthe tracking method and inter-task communication within the trackingsystem), a method which sends speed and steering commands in advance tocounteract any delays is used in accordance with the present invention.The method may be executed as follows:

sense the current position P_(a) ctual

    (Initialization: P.sub.a ctual=P 0!=P 1!=. . . =P d.sub.-- index+1!)

compute error between predicted and sensed position:

    P.sub.e =P.sub.a ctual-P 0! for i=0, d.sub.-- index P i!=P i+1!+P.sub.e

compute the position on the path corresponding to the position of thebeginning of the time interval: get ₋₋ P on (P d₋₋ index!, P_(o) n)

get initial condition:errors(0)=P d₋₋ index!-P_(o) n

compute a quintic polynomial curve in error space predict a position atthe end of the planning time interval;

get₋₋ despos(P_(o) n,^(d) s,P+d₋₋ index+1)

P d+index+1!+=errors(ds)

For example, to compensate a system which has time delays on the orderof two planning intervals (on the order of 250 mSec), the variable d₋₋index is set to 2.0.

Tracking performance improves as the compensation index (d₋₋ index) isincreased to match the delays inherent to the system.

d. Vehicle-ground interaction (VGI):

Reference commands for steering angle and vehicle speed result invarying angualr velocities and accelerations of the vehicle wheels.

VGI describes how the vehicle moves, given steered wheel angles andwheel angular velocities. The principal VGI phenomena are slip angle andunder/oversteer characteristics which are based on the tire/road contactregion geometry, and are affected by tire elastic deformation. Thesephenomena require a larger steering angle as compared to a kinematicallycomputed one.

e. Sensing and actuation timing

Since actual path tracking is controlled by digital processors in thepresent invention, a discrete time interval is used. It is governed bythe position sensing time interval (which may be on the order of 0.25Sec) which is much longer than the computing time required for steeringplanning (which may be on the order of 16 mSec).

At times, especially when the discrete time interval is large, poorpredictions of the vehicle position maybe made which degrade performanceof the tracking method.

FIG. 59 is a diagram illustrating an old sensing and actuation timing.As shown in FIG. 59, the next vehicle position (k+1) is predicted alongwith the steering plan at the end of the position sensing time interval(250 msec).

A compensation method of the present invention, serves to reduce theerror in predicting the next vehicle position by decreasing the discretetime interval. In this method, the vehicle position is predicted for theend of the computing interval (16 mSec) rather than at the end of theplanning interval (250 mSec). The method is executed as follows:

sense the current position P_(a) ctual

    (Initialization: P.sub.a ctual=P 0!=P 1!=. . . =P dindex+1!=P.sub.a ctual,k+1

compute error between predicted and sensed position:

    Pe=Pactual-P.sub.a ctual, k+1 for i=0, d.sub.-- index

    P i!=P i+1!+P.sub.e

compute the position on the path corresponding to the position of thebeginning of the time interval: get₋₋ P_(o) n(P d index!, P_(o) n)

get initial condition: errors(0)=P d₋₋ index!-P_(o) n

compute a quintic polynomial curve in error space

compute a quintic polynomial curve in error space 1!

predict a position at the end of the planning time interval:

get₋₋ despos(P_(o) n, _(d) s, P+d₋₋ index+1)

P d+index+1!+=errors(ds)

predict a position at the next sensing time:

    P.sub.a ctual, k+1=P 0!+(P 1!-P 0!)*(dt.sub.-- plan-dt.sub.-- comput)/dt.sub.-- plan

f. Look-ahead:

Human operators use different look-ahead distances 3310 when driving. Atslow speeds, a driver generally looks at a point on the road relativelyclose to the vehicle, while at higher speeds, this point is generallyfarther ahead of the vehicle. The higher the speed, the farther aheadthe reference point is, resulting in smaller steering corrections.

Thus, in an autonomous application, a look-ahead distance which varieswith speed, logically helps to improve tracking performance.

A desired steering angle may consist of a steering angle from thereference path 3312 and a steering angle 3112 which is computed with aquintic method to correct for tracking errors. These steering angles aresummed to give the vehicle steering command as shown in equation (1)below:

    .0.=.0..sub.ref +.0..sub.error

Note that look-ahead in the autonomous scheme affects only .0.error,even though look-ahead in manual driving affects both the reference anderror compensating steering angles. Shorter look-ahead values result inlarge steering corrections; the look-ahead distance can therefore beinterpreted as a gain in an error feedback system.

An arbitrary model for varying look-ahead distance (L) with speed (V) isexpressed with three parameters, V_(r) ef, L_(r) ef, and slope, as shownin equation (2) below:

    L=slope*(V-V.sub.ref)+L.sub.ref

where V is the speed of a vehicle and L should be between L_(m) in=10and L_(m) ax=30. Tracking performance is improved with the varyinglook-ahead distance 3310 of the present invention.

g. Optimal Control Method

As mentioned above, an embodiment of the present invention uses a modelwhich includes considerations of cornering stiffness, mass and slipangle.

The control problem is formulated as a linear quadratic optimal trackingproblem where the errors in position, heading and curvature areminimized based on the vehicle control model. The optimal path andcontrols are computed from the desired path 3312 and the currentlysensed vehicle position 3304 using the current errors as initialconditions to the optimal control problem.

A few computed steering angles along the initial part of the optimalpath are used as references to the low level steering controller for thenext sensing time interval. This preview optimal steering control hasthe advantage of guaranteeing stability and optimality with respect tothe given performance index. The optimal preview control methodaccording to the present invention, is applicable to the steeringplanning of an autonomous vehicle 310.

The model is derived from a standard telescoped, or bicycle model (notshown) or approximation of the vehicle. The equations describing thevehicle motion inculde terms which represent the VGI described earlier.These equations use the state variables:

    X= x,y,θ, θ!

where x and y represent the global position fo the vehicle; 0 is theheading 3318 of the vehicle, and 0 is the rate of change of heading.

Using these variables, the equations are: ##EQU7##

FIG. 60 is a diagram illustrating the relationship between a coordinatesystem 6010 and a new coordinate system 6020 for the vehicle. Newcoordinate system 6020 is comprised of an axis parallel to the directionthe vehicle is heading and tangent to the path the vehicle is traveling(referred to as a forward direction 6030) and an axis perpendicular tothe forward direction (referred to as a lateral direction 6040). Newcoordinate system 6020 is used to simplify control computations asdiscussed below.

It is well knonwn in the art of optimal control theory, that a costfunction must be selected which is used to minimize selected parametersin the system. The cost funciton used in this problem was selected as:##EQU8##

There are several problems to solve the optimal control problem with thestate equations (14) and the cost function (15);

1. The system is nonlinear. Usually, a two point boundary value problemwhich results from a nonlinear system does not have an analyticsolution. Numerical solutions on the other hand take a long time tocompute.

2. The resulting optimal control problem is a free final time problem.Generally fixed final time problems are simpler to solve than those withfree final time.

3. The first term inside the integration (within the above xostfunction) is the time derivative of the control input, which is notusual in a quadratic cost function of an optimal control problem.However, the time rate change of steering is very important for smoothpath following, because it is directly related to the time rate ofchange of centrifugal force (due to lateral accelerations of thevehicle).

Note that the steering angle is dependent on the curvature of the pathas in FIG. 51.

The following approaches are applied in order to overcome the abovethree problems and to make the resulting optimal control problemtractable:

1. Since the sinusoidal functions in the first and the second equationsof (14) make the system nonlinear, a new coordinate system, an axis ofwhich is parallel to the tangent direction of the corresponding point ofthe path to the current vehicle position, is used. The deviations onlyin the lateral direction are considered in the cost function. These twoapproximations not only eliminate the nonlinearity in the systemequation but also reduce the number of equations to deal with; the firstequation of (14) is not required now. (Refer to "Coordinate Systems.")

2. This problem with free final time, tf, can be converted to the onewith a fixed final value of the independent variable by writing thedifferentials in the equation of motion with respect to the forwarddistance. To this end, a non-dimensional independent variable, s, isdefined as: ##EQU9## 3. To solve the third problem pointed out above, anew state vector and a control input are defined as:

    Xnew= Xold, Uold!.sup.T, Unew=Uold

where Xnew satisfies: ##EQU10##

where Aold and bold denote the old system matrix and the old inputmatrix.

Then, the state variables and the control input are defined as:

    z= ξ,V.sub.ξ,θθ,δ!.sup.T,u=δ (Eq.16)

which satisfies the system equations as the following: ##EQU11##

The new cost function becomes: ##EQU12##

The steering planning with resulting system equation (17) and the costfunction (18) above, can be solved as a linear quadratic trackingproblem as the following. Suppose the system equation and the costfunction are described as ##EQU13## are chosen all symmetric. Then theresulting equations are: ##EQU14##

Thus, the riccati equation (21) must be solved first and the gains arecomputed from the result of the riccati equation and then the forcingfunction driven by the desired path are computed by solving equation(23). Then, the control and states are obtained by solving equation (19)and (24).

The MacFarlane-Potter integration method was tried to solve the riccatiequation. This method is known to be very effective for the steady-statesolution of the time-invariant problem. Since the previewed distance isquite long and the initial parts of the solution are used, this methodseems good to reduce the computation time.

Hence, equation (23) is changed as the following equation (25) andsolved, because the previewed distance is long and only the inertialpart of the solution is used.

    (A-BK).sup.T V+QX.sub.2 =O                                 (EQ.25)

h. Conclusion:

Tracking performance has been improved according to the presentinvention, by investigating and understanding vehicle and control systemdynamics and by designing compensation methods given this understanding.

A degraded performance of a tracking method is attributable to latencyof vehicle control commands, slow system response, and vehicle dynamiccharacteristics. It is possible to counteract each of these effects.

Latency of Vehicle commands, a dominant effect, can be successfullycompensated by modifying the vehicle control hardware and by utilizing amethod which set control commands far enough in advance to compensatethe delays. Decreasing the time lag between when the vehicle position issensed and when the command is issued reduces prediction errors. This isrequired to plan steering angles, and results in better trackingperformance.

Varying look-ahead distance with speed also improves trackingperformance in comparison to using a constant look-ahead distance.

In general terms then, path tracking is the function of staying oncourse. In path tracking in the present invention, as discussed, some ofthe considerations are errors in distance, heading and curvature, delaysin the system including processing delays and delays in vehicle responseto actuators, and so on, dynamic look ahead distance, weighted pathhistory, and extrapolation.

D. Obstacle Handling

1. Introduction

Obstacle handling involves at least three major functions: detection ofobstacles 4002, avoidance of obstacles 4002, and returning to path 3312.The returning to path function is similar to path generation andtracking, as described above.

In addition to path tracking (following), successful navigation ofvehicle 310 requires that vehicle 310 be able to detect obstacles 4002in its path, thus allowing the vehicle to stop or otherwise avoid suchan obstacle before a collision occurs.

In one embodiment of the present invention, a single line infra-redlaser scanner 404 (See FIG. 38) is used in a configuration where thescan is horizontal (not shown). The scan line 3810 does not contact theground, so any discontinuities in the range data can be attributed toobjects 4002 in the environment.

Since a reference path 3312 is available and the vehicle position isknown relative to the reference path, only the range data and a regionbounding the reference path 3312 is processed for threatening objects4002. Objects outside of this region, or boundary zone, are ignored. Thewidth of the boundary zone (not shown) is equal to the vehicle widthplus some selected safety buffer to allow for tracking and positioningerrors. This method is limited in its usefulness and is referred to as"clearance checking."

2. Detection of Obstacles

a. Clearance checking:

In the simplest case of the present invention, the laser 404 may be usedin a single line scan mode with successive range measurements being madeat regular angular intervals as the laser scans over the field of view.Again for simplicity, these scans can commence at regular timeintervals. The term "clearance checking" has been used to describe thismethod. In this version of the present invention, the method has beenlimited to processing only two dimensional data.

This type of obstacle method is limited to checking to see if the path3312 is clear using a single line scan mode with successive rangemeasurements being made at regular angular intervals as the scanner 404scans over the field of view. It does not include any methods toestablish the existence of any obstacle 4002 or to create a path aroundit if the path is not clear. This type of method is not deemed to be aparticularly useful obstacle detection method, except in very rigidlycontrolled environments, such as on a factory floor.

b. Filtering and edge detection scheme:

A second obstacle detection embodiment of the present invention uses amultiple-line scanner 3804 (See FIG. 38), whose scan 3810 contacts theground at some distance in front of the vehicle 310. Since the scan linecontacts the ground, discontinuities in range data can no longer beattributed to threatening objects 4002. For example, profiles fromnatural objects such as hills and banked or crowned roads can causediscontinuities in range data. This technique of the present inventioncan discern discontinuities in range data between threatening objects4002 and natural objects (not shown).

In this embodiment of the present invention, a filtering scheme is usedto decrease the amount of data processed and is independent of thescanner configuration used. The edges of the boundary zone are found bytransferring the range data to an image plane representation 3900 (SeeFIG. 39), where each range value is located by a row number 3908 and acolumn number 3910 (a matrix representation).

Processing load is minimized by selecting a relatively small number ofthe scan lines available in a range image 3900. The scan lines areselected by vehicle speed, and are concentrated at, and beyond, thevehicle stopping distance. The selected scan lines from successiveframes of data can overlap.

In this method, if the vehicle 310 is moving fast, the selected scanlines 3906 are far in front of the vehicle (near the top of the rangeimage 3900). In contrast, when the vehicle is traveling slowly, theselective scan lines 3906 are closer to the vehicle (near the bottom ofthe range image 3900).

Each scan line is made up of many pixels of data. Each pixel has twoparameters associated with it. First, the actual value of the pixel isthe range value returned by the scanner 3804. Second, the location ofthe pixel on the scan line gives an indication of the angle, relative tothe vehicle centerline, at which the range value was recorded. Thiscorresponds to a cylindrical coordinate frame (R,THETA,Z) description.

Given the cylindrical description and the known scanner location withrespect to the vehicle 310, the range values can be coverted to aCartesian coordinate (X,Y,Z) system. The result is a road profiledescription which can be used by a novel filtering scheme to determineif threatening objects 4002 are present in the vehicle path 3812, whileignoring effects due to natural hills and valleys in a typical roadway.

After the scanner data is converted to Cartesian coordinates, the datais processed to determine which part of the scan is actually on the road3312 and which part of the scan line is outside of the vehicle path andtherefore safely ignored. Given the vehicle position and the width of aboundary (which is equal to the vehicle width plus some safety margin),the coordinates of the boundary on either side of the vehicle path canbe determined. The coordinates of the boundary can be compared to thecoordinates of each pixel on the current scan line. The pixels whichhave coordinates outside of the boundary are ignored.

The filtering scheme builds an expectation of the road profile frompreviously sensed road profiles. This expectation is based on threeparameters which were found to adequately describe typical paved roads.These three parameters are:

road crown: the curvature of the road cross section (perpendicular tothe road centerline).

road bank: the `tilt` of the road profile (perpendicular to thecenterline).

road height: the height of the road centerline above a reference planedescribed by the location of the four tires of the vehicle 310.

Expected values of the road crown and the road bank are determined byperforming a standared, least-squares Kalman filtering technique onpreviously sensed scanner data. The Kalman filter basically keeps a typeof running average of the two parameters based on the values determinedfrom the previous data.

In accordance with the present invention, the expected road height for aparticular scan can be determined through one of two similar methods.

One is to average the road height at each pixel within the current scanline to determine a characteristic height of the scan line in question.

The second method is to filter the road height using the standard Kalmanfilter similar to that used when determining crown and bankexpectations.

These three parameters can be used to determine a second order equationwhich describes the expected road profile. This expected profile iscompared to the actual road profile. Any deviations between the twowhich exceed a preset threshold value are assumed to be threateningobjects.

This scheme of the present invention is viable given the assumption thatany detected objects 4002 are small in comparisson the width of theroad. Then, when these averaging or least squares methods are used, theeffects due to objects are negligible in comparisson to natural roaddata.

This filtering scheme also includes a very simple edge detection methodwhich convolves the selected range data with a simple seven pointweighing function.

c. Obstacle extraction:

An additional technique of the present invention processes an entirerange image 3900 from a multi-line scanner 3804 for objects. This methodof the present invention accomplishes three goals:

1. Do not detect obstacles 4002 when none exists,

2. Detect obstacles 4002 when obstacles do exist, and

3. Detect the correct obstacles 4002 when obstacles exist.

Obstacle extraction is obstacle detection through the use of blobextraction. Blob extraction is well known in the art of computergraphics. Obstacles are found by clustering similar pixels into groups,called blobs. The goal of obstacle extraction is to store and processobstacles as units rather than as individual pixels.

The obstacle extraction of the present invention may be done bypreforming the following steps in the image plane 3900:

1. Project the vehicle path into the image plane 3900,

2. Transform range data into height data,

3. Fit a curve to the height at the center of the road (this representsthe expected road height at each row),

4. Threshold the actual road height against the height expectation, and

5. Extract the obstacles (indicated by differences in actual andexpected road heights which exceed the threshold).

(1) Finding the road:

In order to process all the available data, the images 3900 must beprocessed at the frame rate of the scanner 3804. For this reason, mostof the computations in the obstacle extraction method are done in theimage plane 3900. By projecting the path into the image, a large portionof the image can be ignored, and many needless computations avoided.

Assuming that the vehicle path 3812 is specified at regular intervals,the current vehicle position can be used to locate the path segment line3902 in front of the scanner. This path 3812 is transformed from worldcoordinates into image coordinates by projecting the pointscorresponding to the road or boundary edges 3902 into the image plane3900 (see FIG. 39).

A cubic spline is used to interpolate between the gaps. Thus, the centerand edges of the row 3902 are found for each row 3908 in the image 3900.The pixels isolated between the road edges 3902 are converted(cylindrical to Cartesian coordinates) from range to height data. Theoutlying pixels are discarded and not processed any further.

(2) Modeling road height:

Once the center of the road is known for every row 3908 in the image3900, the height for each of these points can be determined. A thirdorder least squares curve is fit to these data.

This has the effect of modeling the general trend of the road (up anddown hills) as well as filtering out the effects of noise and smallobjects lying in the center of the road.

(3) Thresholding:

Obstacles may be located by using a height threshold. A straight heightthreshold would be meaningless since the surrounding terrain is notnecessarily flat. Hence, the threshold is referenced against theexpected height, as predicted by the third order fit, at the row number3908 of the given pixel.

In this manner, a hill is not considered an obstacle since the heightexpectation and the actual height should match very closely. On theother hand, a real obstacle 4002 would barely reflect the expected roadheight (due to the least squares fit), and therefore is readily found bythresholding. The result of this thresholding is a binary image (notshown) suitable for a "blob extraction." The binary image only indicateswhere an object is or is not present in the image.

(4) Blob extraction:

Blob extraction works by clustering adjacent set pixels (indicating anobstacle 4002 is present) together and treating them as a unit. Twopixels are adjacent if they are either:

1. In the same column 3910 and have consecutive row numbers 3908, or

2. In the same row 3908 and have consecutive column numbers 3910.

By grouping pixels together into blobs, the obstacles 4002 can betreated as a whole unit and are suitable for further processing.

(5) Applications:

One way to use extracted blobs is to pipe them as input into anotherprogram. For example, the objects 4002 can be parsed into coordinatesand used to accumulate a global object map 4004 (See FIG. 40). This map4002 is then passed into another program, and used to do collisionavoidance or path planning.

3. Avoidance of Obstacles

Once the present invention detects an obstacle 4002 in the path of thevehicle 310 (See FIG. 40), it must then avoid a collision with theobject. Certain assumptions are made concerning the obstacle avoidanceproblem:

1. The obstacle environment is populated with obstacles 4002 that can berepresented by convex-polygons or convex lines;

2. The navigation methods only have access to the local environmentinformation in the form of a local map representing all of the visiblefaces of the obstacle from the position of the vehicle 310, which can beobtained from unprocessed laser range data or from data processedthrough blob-extraction;

3. The vehicle 310 is a conventionally steered type which hasconstraints on its speed and acceleration and constraints on itssteering angle and the rate of change in the steering angle.

To deal with the obstacle avoidance problem, the present inventiondivides it into two sub-problems.

First, to decide if any obstacles are in the way, and if so, which sideshould the vehicle pass on. Then select a sub-goal 4006, which will leadthe vehicle 310 around the obstacle 4002, leading towards a higher levelgoal 4008, which is to get back on the desired path.

Second, once a sub-goal 4006 is selected, make a steering decision whichdrives the vehicle 310 towards the sub-goal 4006, while steering clearof the obstacle 4002. A sub-goal selection method and a steeringdecision method of the present invention solve these two sub-problems.

The above enumerated assumptions are managed in the following process:

The obstacle locations are obtained from the laser range scanner 3804 or404. The range data generated by the scanner 3804 or 404 are processedto produce a list of polygonal faces, modeling the visible portions ofthe obstacle 4002 from the vehicle position. Each time new range databecome available, a sub-goal selection method is executed to generate asub-goal 4006 and determine regions of safe navigation (free-space 4010)for the steering decision method. The frequency at which the sub-goalselection method can be executed depends on the rate at which thescanner 3804 or 404 can collect data. The achievable vehicle speed, inturn, depends on this frequency of execution.

For the steering decision method, a higher sampling rate is desirable inorder to produce a smooth path. Therefore, the steering decision methodis executed more frequently than the sub-goal method.

The basic flow of the sub-goal method is the following:

    ______________________________________    1          save last initial-subgoal, subgoal, and free-               space, set goal.sub.-- blocked flag to true;    2          if final goal is visible               generate direct goal               if direct goal is visible                 set goal.sub.-- blocked flag to false;    3          otherwise               generate an initial subgoal               set subgoal to initial subgoal               recursively generate subgoals until the               latest one is visible. if subgoal not               feasible, abort;    4          if goal.sub.-- blocked flag is true               restore old initial-subgoal, subgoal,               and free.sub.-- space;    5          otherwise               generate free-space               if free-space is not safe                 restore old initial-subgoal,                 subgoal, and free.sub.-- space.    ______________________________________

Sub-goal Method: First (step 1 above), the initial-subgoal, subgoal, andfree-space generated from the previous iteration is saved. This assuresthat when the newly generated subgoal is not safe, the old subgoal cancontinue to be pursued.

Next (step 2 above), when the final goal is visible, attempt to generatea direct goal which is not associated with any obstacles 4002. Althoughthe final goal is visible in the local map, it does not necessarily meanthat no obstacle is blocking the final goal because obstacles outsidethe scanner range (both distance and angular wise) will not berepresented in the local map. Therefore, when generating a direct goal,ensure that the goal is located in the cone area which is covered by thescanner 3804 or 404 to avoid placing a subgoal on or behind an obstacle4002 that is not in the local map.

The next step (step 3 above) handles the situation where the final goalis blocked by an obstacle 4002 in the local map. In this case, theobstacle 4002 that blocks the line of sight to the final goal is firstdetermined.

Given a blocking obstacle, there are two possible ways of going aroundit. If both edges of the obstacle are in the range of the scanner 3804or 404, we may choose to go around the edge which gives the minimum sumof the distances from the vehicle 310 to edge and from the edge to thefinal distance. If only one edge of the obstacle 4002 is in the range,choose that edge to go around. If none of the edges is visible, alwaysarbitrarily choose the left edged to go around. Once the edge to goaround is determined, place the initial subgoal away from the edge at adistance that is proportional to the vehicle size.

Because of this displacement, the resulting subgoal may be blocked byother obstacles 4002. This calls for the recursive generation of subgoalon the obstacle, which blocks the line of sight to the subgoals justgenerated. This recursive process continues until a subgoal visible tothe vehicle 310 is generated. Each subgoal so generated is checked forviability. By viability it is meant that the subgoal does not lead thevehicle 310 towards a gap between two obstacles 4002 which is too smallfor the vehicle to pass through. When such a condition is detected, thevehicle 310 will stop.

The direct subgoal generated in the second step (step 2 above) couldpossibly be obscured from the vehicle 310. If such is indeed the case,the old subgoals from the previous iteration is restored and used next(step 4 above).

In the final step (step 5 above), generate the free-space 4010 for thevisible subgoal, which is a triangular region that contains noobstacles. Once the free-space 4010 is generated, the safeness of thesubgoal and free-space 4010 can be determined. When the new subgoal andfree-space 4010 is not safe, the old subgoal and free-space is againretained. Otherwise, the new subgoal and free-space is used.

The steering decision method of the present invention is composed of twomajor components: transferring state constraints to control constraints;and determination of the desired control vector.

Once the control constraints and the desired control vector arecomputed, the control vectors can be determined using optimizationtechniques well known in the art.

4. Return to Path

The present invention includes a method, as shown diagramatically inFIG. 40, whereby a safe path around a detected object 4002 will beplotted and navigated so that the vehicle 310 will reacquire thereference path after avoiding the object 4002.

5. Scanner System

a. Introduction:

Referring to FIGS. 38 and 42, the present invention also includes alaser scanner system 404. The scanner 404 is used to find obstructions4002 (See FIG. 40) that randomly crop up in the vehicles 310 path, aspreviously discussed.

Sources of such obstructions 4002 may be varied and numerous dependingon the particular work site. They may include fallen trees and branches,boulders, moving and parked vehicles, and people.

The scanner 404 gives the autonomous vehicle 310 the ability to detectand deal with the external world as conditions require.

b. LASER Scanner:

The major components of the laser scanner system 404 are depicted inFIG. 42.

A laser range finder 3804 uses an infra-red beam 3810 to measuredistances between the range finder unit 3804 and the nearest object4002. A brief pulse is transmitted by the unit 3804 and the time for thebeam 3810 to reflect off an object 4002 and return gives the distance.

The beam 3810 from the range finder 404 is reflected by a rotatingmirror 4222 giving the range finder 404 a 360° view of the world. Mirrorrotation is accomplished through a motor 4206. The motor speed iscontrolled via a terminal 4210, which communicates with a motoramplifier/controller 4220 through a standard RS232C serial link 4224.Synchronization between laser firings and mirror angular position isdone with an encoder.

Distance data on a line 4226 from the laser range finder 404 is taken byan interface circuit 4228, which transmits the data differentially to abuffer circuit 4214. Individual pieces of data are collected by thebuffer circuit 4214 until the mirror 4222 makes one full revolution.This set of data comprises one scan. When a scan is complete, the buffercircuit 4214 signals a processor 4212, whereupon data for the entirescan is transferred to the processor 4212 for processing.

c. Scanner System Interface:

The interface circuit 4228 has three functions.

First, it acts as a safety monitor. A situation could occur where themirror 4222 would stop rotating, as in the case of the drive belt 4230between the motor 4206 and mirror 4222 breaking. Under this condition,the laser 4204 would continue to fire, and since the mirror 4222 isstationary, it would fire at a single point (dangerous for anyonelooking directly into the laser beam). The interface circuit 4228,however, senses when the angular velocity of the mirror 4222 falls belowhalf a revolution per second, and disables the laser 4204 if such acondition occurs.

The second function is to disable the laser 4204 from firing for part ofthe 360 degrees scan area. Typically, the laser scanner unit 404 will bemounted in front of a vehicle 310, and the field of interest is in the180 degree area in front of the vehicle. The vehicle itself will blockthe back portion of the 360 degree scan area. In this case, thecircuitry 4228 will prevent the laser 4204 from firing into the vehicle,extending the life of the laser diode while receiving range data for thearea in front of the vehicle. The enabling and disabling of the laserrange-finder 4204 is done through two sensors (not shown) mounted nearthe mirror housing 4222. For testing purposes, or for applications wherea 360 degree scan is desirable, the disable feature can be turned offthrough a DIP switch.

The third function of the circuit 4228 is to convert signals betweensingle ended and differential form. TTL signals from the laser unit 4204are differentially transmitted to the buffer circuit 4214, anddifferentially transmitted signals from the buffer circuit 4214 areconverted to TTL levels. This prevents noise contamination along thecable 4226 connecting the two circuits.

d. Scanner System Buffer Circuit:

The function of the buffer circuit 4214 is to synchronize laser 404firings with the angular position of the mirror 4222, to collect datafor one complete scan, and to transmit the scan to computer 4214 forprocessing.

The angular position of the mirror 4222 can be determined throughsignals sent by the encoder 4208. The buffer circuit 4214 uses twosignals from the encoder 4208: the Z and A channels.

The Z channel is the encoder index; it gets asserted once per revolutionof the encoder 4208, and is used to signal the beginning of the scanarea.

The A channel is one line of the two line quadrature output of theencoder 4208, and pulses 1000 times per revolution of the encoder. Thischannel is used to trigger laser firings.

One additional signal is needed to fully synchronize the scan field withthe encoder signals. There is a gearing ratio of 2:1 between theencoder/motor 4206 and the mirror 4222. Two revolutions of the encoder4208 rotates the mirror 4222 once. This translates to 2Z channel plusesand 2000 A channel pulses per revolution of the mirror 4222, and theinability to differentiate the beginning of the first half of the scanwith the beginning of the second half.

To fully synchronize the scan field, the DB (dead band) signal generatedby the interface circuit 4222 is used. The DB signal, used to disablethe laser 4204 from firing in the back half of the scan, allows thedifferentiation of the front and back halves of the scan. The Z and DBsignal together signal the beginning of the scan area.

The second task of the buffer circuit 4214, to collect data for onecomplete scan, is accomplished through the A channel of the encoder4208. The 2000 pulses of the channel is divided by either 2, 4, 8, or16, selected through DIP switches (not shown) on the circuit board 4228.This allows the number of data points per scan to be varied between1000, 500, 250, and 125. The divided signal is used to trigger the laserrange-finder 4204 at appropriate angular intervals, and to store theresulting range data in memory 4214.

The sequence of events is as follows. W (write) is asserted one clockcycle upon a rising edge on the divided A signal. At this point, datafrom a previous T (laser trigger) is available and is stored in memory4214. T is asserted the following clock cycle, triggering the laser andputting the resulting range data onto the memory input bus 4226. Thisdata is written on the next W pulse, repeating the cycle.

The final task of the buffer circuit 4214 is to transmit the scan datato a computer 4212 for processing. Completed scans are signaled by the Zand the DB signals (the beginning of a scan is also the end of aprevious one). Upon a completed scan, an interrupt request line isasserted, and remains asserted until either the mirror 4222 has madehalf a revolution, or the processor 4212 acknowledges the interrupt. Inthe first case, the half revolution of the mirror 4222 is signaled by asubsequent Z pulse and indicates a timeout condition; the processor 4212has failed to respond and the data is lost.

In the normal case, the interrupt is acknowledged. Upon receipt of theacknowledgement, STR (data strobe) is asserted and held until IBF (inputbuffer full) is received. During this time, data is put on the data bus4230 and may be ready by the computer 4212. Data is valid on the bus4230 until IBF is asserted, at which time STR is de-asserted and thedata removed from the bus 4230. Once the processor 4212 detects thede-assertion of STR, it de-asserts IBF. This causes STR to be assertedfor the next piece of data, repeating the cycle.

Scan data is collected and stored in two memory banks 4214. This avoidsshared memory and synchronization problems between scan storage and scantransmission. Data for a new scan is stored in one bank, while theprevious scan is being transmitted from the other bank.

The buffer circuit 4214 removes from the processor 4212 theresponsibility of synchronizing laser findings with mirror position andcollecting individual pieces of data. It allows more efficient use ofCPU time, as data is received in scan sized chunks. The processor 4212spends its time processing the data, not in collecting it.

E. Vehicle Controlling Systems

1. Introduction

Referring now to FIG. 43, the vehicle controls are comprised of four,low-level functional blocks. One is called a "vehicle manager" 4302. Asecond is called a "speed control" 4304. The third is called a "steeringcontrol" 4306. The fourth is called a "monitor/auxiliary control"(depicted as two separate blocks 4310 and 4308. These are described inturn below.

They are all tied together with a high-speed serial data bus 4314. Thebus 4314 is a data collision detection, packet passing system.

Each of these functional blocks have separate microprocessors, forinstance of the Motorola 68000 16 bit series. Each of thesemicroprocessors talks to and listens to the others over the bus 4314.

While each functional block has a more or less specific function, thevehicle manager 4302 functions as a communications hub. It sends to andreceives messages from the navigator 406 via an RS-422, 9600 Baud seriallink 4316. It is also listening to and sending to the remote control or"tele" panel 410 via an FM radio communications link 4318.

2. Vehicle Manager (modes)

As mentioned above, the vehicle manager 4302 receives commands from aremote control panel 410 and the navigator 406. It then decides whichmode "A, M, T, or R" (for Autonomous, Manual, Tele, or Ready) thevehicle 310 should be in.

a. Ready Mode

Reference is now made to FIG. 44, which shows the states (modes) and howthe vehicle 310 changes between states. The navigator 406 cannot set themode itself. Notice that the vehicle 310 cannot change from tele toauto, for instance, directly. It must pass through the ready mode 4404first in that case.

The ready mode 4404 brings the vehicle 310 to a stop in a known state.This is because it would be difficult to make a smooth transition, from,for instance, auto mode 4408 to tele mode 4406 while the vehicle 310 wasmoving. The tele control panel joystick 4502, 4504 would have to be injust the right position when control was switched.

Going from tele 4406 to auto 4408 mode, there is the consideration thatthe navigator 406 must initialize. For example, it must determine whereit is with respect to a route before taking control, which takes somefinite time, during which the vehicle 310 might otherwise drive offuncontrolled.

b. Tele Mode

Tele control mode 4406, also referred to as tele-operation, remotecontrol or radio control mode, provides a way of controlling the vehicle310 from a remote location while the vehicle 310 is kept in view.

Shop personnel would use the tele-operation mode 4406 to move thevehicle 310 in the yard, for example. Advantageously, this mode wouldalso be used by a shovel or loader operator to maneuver the vehicle intoposition for loading or unloading, and moving the vehicle into alocation where autonomous mode 4408 would resume control.

In tele-operation mode 4406, each vehicle 310 at an autonomous work site300 would have its own unique identification code that would be selectedon a radio control panel 410 to ensure communication with and control ofthe correct vehicle only. The vehicle 310 would only respond totele-operation commands 4318 when its unique identification code istransmitted. Any conflict between modes, such as between manual 4402 andtele 4406, would be resolved in favor of manual mode 4402, for obvioussafety reasons.

The navigator 406 keeps track of where the vehicle 310 is while beingoperated in the tele mode 4406, even though, in tele mode, the vehiclecan be maneuvered far off of a known route.

c. Manual Mode

Manual control mode 4402 may be required when the vehicle 310 is beingmaneuvered in very close quarters, for example, at a repair shop,equipment yard, and so on, or when a control subsystem needs to beremoved for repair or maintenance.

This control mode may be implemented to be invoked whenever a humanoperator activates any of the manual controls. The simple action ofstepping on the brakes 4708, moving the shift lever from somepredetermined, autonomous mode position, or grasping the steering wheel4910, for example, would immediately signal the control system thatmanual control mode 4402 is desired and the system would immediately goto the manual mode.

While in manual mode, the autonomous system would continuously monitorvehicle motion and maintain an updated record of the vehicle position sothat when and if autonomous mode 4408 was desired, a quicker and moreefficient transition could be made.

When autonomous mode 4408 is again desired, the human operator wouldthen affirmatively act to engage autonomous mode 4408, by physicallymoving a switch or lever, for instance, to the autonomous control mode.A time delay would preferably be built in so that the human operatorwould have the opportunity to leave the vehicle 310 if desired. At theend of the time delay, the system would then give several levels ofwarning, such as lights, horn, or the like, indicating autonomoustakeover of the vehicle 310 was imminent.

d. Autonomous Mode

The autonomous mode 4408 is entered into from ready mode 4404. In theautonomous mode 4408, the vehicle 310 is under the control of theautonomous navigation system.

In this mode, the vehicle control system receives messages from thenavigator 406 as discussed above, through the vehicle manager 4302. Thevehicle manager 4302 is, as discussed, basically the communications andcommand hub for the rest of the controllers.

The vehicle manager 4302, and the other functional control blocks, allcommunicate with the shutdown circuits 4312 as well. The shutdowncircuits 4312 are discussed in more detail below.

3. Speed Control

The speed control subsystem 4302 may be organized to contain a speedcommand analyzer, closed loop controls 4800 for the engine 4614,transmission and brakes 4700, 5000, a real time simulation model of thespeed control system, and a monitor 4310 that is tied to an independentvehicle shutdown system 4312. It is designed to be placed in parallel tothe production system on the vehicle 310.

The speed control functional block 4304 takes care of three basicfunctions. It controls the governor on the engine 4614. It controls thebrake system 4606. And it controls the transmission 4610 via theproduction transmission control block 4616.

The production transmission control block 4616 is interfaced with thespeed control block 4304 in a parallel retro-fit of the autonomoussystem onto the production system as shown in FIG. 48. The productiontransmission control block 4616 is a microprocessor based system whichprimarily monitors speed and shifts gears accordingly.

The autonomous system speed control block 4304 feeds the transmissioncontrol block 4616 the maximum gear desired. For instance, if thevehicle 310 is to go 15 mph, the maximum gear might be third gear. Theproduction transmission control block 4616 will control all the shiftingnecessary to get to that gear appropriately.

The governor 4626 (FIG. 46) controls the amount of fuel delivered to theengine 4616. Thus, it controls engine speed. The autonomous system iscapable of being retrofitted in parallel with the production governorcontrol system, in a similar fashion as described with respect to thetransmission system.

The brake system is shown in FIGS. 47 and 50. The autonomous system hereis also capable of being retrofitted to the production brake system.

The following discusses vehicle systems shown in FIGS. 46, 48, 47, 50and 49. These systems relate to the vehicle drive train 4600 andsteering 4900 systems.

Referring to FIG. 46 a governor 4626 controls engine speed 4222, whichin turn controls vehicle speed 4624. The engine power is transferred tothe drive wheels through the drive train 4600 which is comprised of:

torque converter 4612

transmission 4610

final drive 4608

brake system 4606

wheels 4604

The function of these systems is well known in the art.

Several key systems were modified in accordance with the presentinvention to effect autonomous control. The primary systems were thespeed control (engine speed, transmission, vehicle speed, and brakes)and steering systems. Each key system is design with manual overridecapability as a safety measure. In all cases, manual control haspriority so that if the vehicle is operating autonomously, and anoperator takes control of any one of the vehicle functions, controlautomatically is returned to the operator.

The system also provides an emergency override button (not shown; alsoreferred to as a `panic` button) which, when activated, disables allelectronicly controlled systems and returns the vehicle 310 to manualcontrol 4402.

The system also provides for sensing the pneumatic pressure which is akey part for actuating some of the key systems. If this pressure fallsbelow some preset threshold, it is assumed that there is a problem andthe vehicle control system reverts to manual control 4402 and thevehicle 310 is stopped.

FIG. 48 depicts the system used to control engine speed. This systemuses electronicly controlled valves 4808 and 4812 to regulate pneumaticpressure in parallel to a pedal 4806 which can be manually operated tooverride electronic control of the engine speed 4622. The pressuresensor 4802 and the engine speed sensor 4622 provide the necessaryfeedback for the electronic speed control system 4304.

Also required to control the vehicle speed is a transmission control4616. The basic control system is readily available on the particularvehicle used for this purpose.

In addition to controlling the engine speed 4622 as a means ofregulating vehicle speed, it is also necessary to control the vehicleservice brakes 4606. This system is shown in FIG. 47 and is necessary toeffect normal stoppage or slowing of the vehicle 310. This system useselectronicly controlled pneumatic valves 4712 and 4716 in parallel witha manually operated brake pedal 4708 and/or retarder lever 4710 toregulate the braking force. These two manual inputs can override theelectronic control system when actuated. The pressure sensor 4702 andthe vehicle speed sensor 4624 provide the necessary feedback to regulatethe braking force.

Control of vehicle steering is also required for the vehicle to operateautonomously. The system which performs this function is shown in FIG.49. The system consists of a Rexroth proportional hydraulic valve 4912which can be actuated electronically to provide flow to hydrauliccylinders 4914 and 4916 attached to the vehicle steering linkage. Thesystem also comprises a manually operable hand-metering unit, or HMU,4918, which is in parallel to the electronicly controlled system. Themanual system can override the electronic system, if required, as asafety measure. Also, the system provides a switch 4920 on the HMU todetect when the manual steering wheel 4910 is different from thecentered position. When not centered, the autonomous system assumes thatthe system is being operated manually 4402 and disables autonomouscontrol of the vehicle 310.

Electronic control of the vehicle parking brake is also included as anadded safety feature. This system is shown in FIG. 50. For properoperation under autonomous control, the parking brake is manually placedin the `ON` position. When the vehicle proceeds through the status modes(MANUAL 4402, READY 4404, and AUTO 4408), the parking brake isautomatically released by electronically controlling the pneumatic valve5008. This system is in parallel to the manual systems comprised of thebrake lever release valve 5016 and the Emergency brake lever 5014.

When a problem is encountered, the vehicle 310 is automatically placedunder manual control. Since the manual setting of the park brake isnormally `ON`, this activates the parking brake, stopping the vehicle310 as quickly as possible.

4. Steering Control

Referring again to FIG. 43, the steering control functional block 4306is responsible for controlling the steer angle of the vehicle's wheels.It sends out commands to a valve 4912 to control the steer angle andreceives information from a resolver (not shown) mounted on the tie rodsystem, so that it knows what the actual wheel angle is.

The steering angle can be controlled with an accuracy on the order of ahalf a degree, and the resolver is accurate to something less than that,on the order of an eighth of a degree.

At some point in the useful life of the vehicle 310 the resolver may goout of adjustment. If this happens, the vehicle will not be able totrack the path 3312 properly.

However, the navigator 406 constantly monitors the vehicle 310 todetermine how far the vehicle 310 is from the desired path 3312. (Thevehicle 310 is always off the desired path 3812 to some extent, and thesystem is constantly correcting.) If the vehicle 310 is more than acertain distance, for example several meters, from the desired path3312, the navigator 406 stops the vehicle as a safety precaution.

The steering control system 4306 itself is also always checking to makesure the resolver is accurate, and that steering commands 420 receivedhave not been corrupted (not shown) by noise or other error sources. Asteering simulation model may also be implemented as an additional checkof the system.

The autonomous steering system 4900 may be designed to be implemented inparallel with a manual steering system, and can be retrofitted on to thevehicle 310 in a similar manner as the speed control system.

As shown in FIG. 49, the existing or production manual steering systemhas a manual steering wheel 4910 which turns a hand metering unit, orHMU 4918. The HMU 4918 controls a valve 4912 which controls flow ofhydraulic fluid to steering cylinders 4914, 4916, which turn the wheels(not shown).

A switch 4920 on the HMU 4918 detects off-center position of thesteering wheel 4910 as an indication to change to manual control ofsteering. An operator riding in the cab can merely turn the steeringwheel 4910 to disable autonomous steering control 4408.

Under autonomous steering control 4408, the manual steering wheel 4910in the cab remains centered no matter what position the autonomoussteering control has turned the wheels to. There is no mechanicallinkage between the steering wheel 4910 and the wheels themselves.

Of course a vehicle 310 may be manufactured without any manual steeringsystem at all on the vehicle if desired. To drive the vehicle manually,the tele-panel 410 could be used, or some sort of tele-panel might beplugged into the side of the vehicle 310 to control it without a radiolink 4506 in close quarters, for instance. A jump seat might be providedfor an operator in such situations.

Some discussion of the steering model developed may facilitate a betterunderstanding of the present invention.

a. Steering Model

The basis for the steering planner is a tricycle steering model shown inFIG. 5.1. This model permits the calculation of the required steer angleindependent of the velocity of the vehicle.

    .0.=tan.sup.-1 LC path

To use this model, the desired path 3312 must contain the curvature ofthe path to be followed. The curvature is the inverse of theinstantaneous radius of curvature at the point of the curve.

FIG. 61 is a diagram illustrating the relationship between the desiredpath 3312 and the curvature of the path. FIG. 61 shows desired path3312, a point 6110 where an instantaneous radius of curvature 6120 is bedetermined, and a circle 6130 defined by instantaneous radius ofcurvature 6120. The curvature is the inverse of the instantaneous radiusof curvature 6120 at the point 6110 of the desired path 3312.

This is also equal to the second path derivative at the point.

b. Path Representation

Referring to FIGS. 22-34, the response of autonomous vehicle 310 intracking a path 3312 depends partly on the characteristics of the path3312. In particular, continuity of the curvature and the rate of changeof curvature (sharpness) of the path 3312 are of particular importance,since these parameters govern the idealized steering motions to keep thevehicle 310 on the desired path 3312. In the case where a path 3312 isspecified as a sequence of arcs and lines, there are discontinuities ofcurvature at the point where two arcs of differing radii meet.Discontinuities in curvature are troublesome, since they require aninfinite acceleration of the steering wheel. A vehicle travellingthrough such transition points with non-zero velocity will experience anoffset error along the desired path 3312.

In general, and as shown in FIG. 33, if a posture 3314 is desired as thequadruple of parameters--position 3320, heading 3318, and curvature 3316(x, y, 0, c), then it is required that the path 3812 beposture-continuous. In addition, the extent to which steering motionsare likely to keep the vehicle 310 on the desired path 3312 correlateswith the linearity of sharpness of the path, since linear curvaturealong a path means linear steering velocity while moving along the path.

Certain spline curves guarantee posture continuity. However, thesespline curves do not guarantee linear gradients of curvature alongcurves. Clothoid curves 2602 have the "good" property that theircurvature varies linearly with distance along the curve. Paths composedof (a) arcs and straight lines or (b) clothoid segments have beendeveloped.

A path that has discontinuities in curvature results in larger steadystate tracking errors. This is particularly the case when the actuatorsare slow.

The path representation must contain sufficient information to calculatethe steer angle 3112 (See FIG. 31) needed to drive the desired path3312, that is, it must consist of at least the position, heading,curvature and speed. A position on the desired path 3312 has beendefined as a posture 3314, and the structure of a posture in the presentinvention is given by:

    ______________________________________    c. Posture Definition    ______________________________________    North:  desired north coordinate    East:   desired east coordinate    Heading:            desired heading    Curvature:            desired curvature    Speed:  desired ground speed    Distance:            distance between current posture and the previous    ______________________________________            posture.

d. Position Information

The position information 3322 is obtained from the vehicle positioningsystem (VPS) 1000 and is, for example, 71 bytes of data. The structureof the information used to track the desired path 3312 is a subset ofthe 71 byte VPS output and is given by the VPS short definition shownbelow.

    ______________________________________    e. VPS Short Definition    ______________________________________    Time:         gps time    North:        wgs 84.sub.-- northing    East:         wgs 84.sub.-- easting    Heading:      compass direction vehicle is moving    Curvature:    calculated from other variable    N.sub.-- velocity:                  north velocity    E.sub.-- velocity:                  east velocity    Yaw rate:     rate of change of the heading    G.sub.-- speed:                  ground speed    distance traveled    ______________________________________

f. Steering Method

The steering planner calculates the steer angle needed to follow thedesired path. If the vehicle 310 was on the desired path 3312, the steerangle is:

    ON PATH .0.steer=f(Cdesired)=tan .sup.-1 LC

If the vehicle 310 is off the desired path 3312, then the steer angleis:

    OFF PATH .0.steer=f(Cdesired+Cerror).

The method of the present invention used to calculate Cerror is aquintic method. The quintic is a fifth degree polynomial in an errorspace that defines a smooth path back to the desired path 3312. Thedegree of the polynomial is defined by the needed data, that is, Cerrorand the known end constraints.

FIG. 62 is a diagram illustrating an error curve 6210 between the smoothpath of the quintic polynomial and the desired path 3312. The errorcurve 6210 shows that the vehicle will smoothly return to the desiredpath 3312 (or nearly so) within a lookahead distance (L) 6220.

Polynomial in error space is:

    error(s)=a.sub.0 +a.sub.1 s+a.sub.2 s.sup.2 +a.sub.3 s.sup.3 +a.sub.4 s.sup.4 +a.sub.5 s.sup.5 |.sub.o.sup.L

    error'(s)=a.sub.1 +2a.sub.2 s+3a.sub.3 s.sup.2 +4a.sub.4 s.sup.3 +5a.sub.5 s.sup.4 |.sub.o.sup.L

    error"(s)=2a.sub.2 +6a.sub.3 s+12a.sub.4 s.sup.2 +20a.sub.5 s.sup.3 |.sub.o.sup.L.

error (0) position=current desired position-current actual position

error' (0) heading=current desired heading-current actual heading

error"(0) curvature=current desired curvature-current actual curvature

at s=L (L=lookahead distance):

error (L) position=0

error (L) heading=0

error (L) curvature=0

The coefficients of the polynomial error(s) are functions of L, thedistance at which the errors go to zero:

    error (0)=a.sub.0

    error'(0)=a.sub.1

    error"(0)=2a.sub.2

    error (L)=a.sub.0 +a.sub.1 L+a.sub.2 L.sup.2 +a.sub.3 L.sup.3 +a.sub.4 L.sup.4 +a.sub.5 L.sup.5

    error'(L)=a.sub.1 +2a.sub.2 L+3a.sub.3 L.sup.2 +4a.sub.4 L.sup.3 +5a.sub.5 L.sup.4

    error"(L)=2a.sub.2 +6a.sub.3 L+12a.sub.4 L.sup.2 +20a.sub.5 L.sup.3

These five equations are solved symbolically for the coefficients a₀, a₁. . . a₅. Then, each coefficient can be easily determined for anyreasonable set of boundary conditions.

Once the coefficients of the polynomial are obtained, the error"(s) canbe evaluated for some picked₋₋ s, which corresponds to a distance alongdesired path from s=0 and is presently defined as:

    s.sub.picked =ground speed*planning interval

to obtain the correction term:

    Cerror=error"(s.sub.picked).sub.curvature

to calculate the new steer angle:

    .0.steer=tan .sup.-1  (Cdesired+Cerror.sub.@spicked)L!

This calculation is done at each planning interval which is presently0.25 sec. (dt₋₋ plan).

5. Monitor/Auxiliary

Referring now to FIG. 43, the monitor/auxiliary functional block(s) 4308and 4310 take care of some miscellaneous functions not performed by theother blocks of the vehicle control system. For instance, start or killthe engine 4616, honk the horn, raise or lower the bed, setting theparking brake on or off, turning the lights on or off, are some of itsfunctions.

The monitor block 4310 also checks the commands that are being sent byor to the other functional blocks on the bus 4314 to see if they arevalid. If error is detected, it will signal the shutdown circuits block4312 and the system will shutdown as discussed below.

6. Safety System (Shutdown)

a. Introduction

The safety system, including shutdown circuits 4312, (see FIGS. 43 and52) operates to stop the vehicle 310 on detection of a variety of errorconditions by setting the parking brake on. This results in the vehicle310 coming to a safe stop in the shortest distance possible.

Since the parking brake is designed to be normally "set" or "on," andthe electronic circuits operate to release it, upon a failure of theelectronic controlling system(s) the power 5216 is turned off to theactuators 5006, so that there is no power to actuate valves, and theparking brake returns to its normal position, called "set."

Whenever several erroneous commands are received, or whenever the speedand/or steering simulation models disagree beyond an acceptabletolerance with vehicle sensor outputs 4622 and 4624, are examples ofconditions which could result in shutdown of the system. The shutdownsystem 4312 is an independent and separate subsystem from the otherautonomous control subsystems (see FIGS. 43 and 52).

b. Shutdown Control

The safety system shutdown circuits 4312 shown in FIG. 43 connected toreceive the outputs of the other vehicle control system functionalblocks is shown in more detail in FIG. 52.

It is a fail-safe type design. It contains no microprocessor at all. Itis all hard-wired, discrete logic.

A feature of the vehicle control system 4312 design is that allfunctional blocks are capable of detecting errors in the output of theothers on the serial bus 4314. So if one of them senses that another isnot functioning correctly, it can send a signal to the shutdown circuits4312 to shut the system down.

For example, the speed and steering blocks each look at their receivedcommands (received via the vehicle manager 4302) to make sure they arevalid. They also make sure that what they are told to execute, that is,what they are requested to command, is within predetermined bounds. Ifnot, they will act to shut the system down.

The safety system may also be monitoring oil, hydraulic and pneumaticpressures, and temperatures, for instance, making sure they aresufficient to safely operate and control the vehicle.

The safety system includes switches for manual override, including apanic stop 5208, switches on the brake pedal 5202 and steering wheel5206.

7. Bus Architecture

The bus 4314 that inter-connects the vehicle control system functionalunits 4302, 4304, 4306, 4308, and 4310 is a serial data type common busimplemented in a ring structure using a data packet collision detectionscheme.

F. Functional Descriptions/Methods

1. The NAVIGATOR

The following is a description of the navigator 406, shown in FIG. 53,titled TASK DIAGRAM. Each of the tasks diagrammed is discussed below.

a. MAIN (executive)

In the center of FIG. 53 is a task labelled "main (exec)" 5316. Thistask 5316 coordinates inter-task communications and performs high leveldecision making for the navigator 406. One of the primary decisions thetask 5316 makes is when to (dis)engage the tracker 5306, based onmessages received from the other tasks in the system.

b. Monitor₋₋ veh Status

This task 5308 is shown above and to the right of the "main" task 5316.It functions to read the vehicle port 5326, and report vehicle modechanges and navigator-to-vehicle communication state to the "main" 5316via the EXEC QUEUE 5328. Additionally, the status of the vehicle 310 iswritten to a global memory structure 5400 (see FIG. 54).

c. Scanner

Shown in the lower right-hand corner of the task diagram FIG. 53 is thescanner task 5310, which provides for communication to the "main" 5316of data from the obstacle detection system 404.

d. Console And Console₋₋ parser

The console 5312 and the console-parser 5314 are shown just below the"main" task 5316 in the task diagram FIG. 53. These tasks were developedas a debugging tool during the development of the system. They displayand manipulate navigator 406 states according to user input from aterminal 5302. The console₋₋ parser task 5314 also is used to settracker parameters.

e. Get₋₋ directives

This task 5320 is shown in the upper left-hand corner of the taskdiagram FIG. 53. It is part of the host-navigator interface 5330.Messages from the host 402 are received and decoded by this task 5320.Then, depending on the message, the message is either communicated tothe "main" task 5316, or to another task. This other task would thenformulate an appropriate response from the navigator 406 to the host402.

f. Msg₋₋ to₋₋ host

This task 5318, shown just above and to the left of the "main" task5316, formulates messages from the navigator 406 to the host 402 andcommunicates them to the host 402.

g. Vps₋₋ position

This task 5322 is shown at the left side of the task diagram FIG. 53.The vps₋₋ position task 5322 reads the (20 Hz) output from the VPSsystem 1000. The data is checked for correctness (for example,"checksum") and if correct, it is put into a global memory structure5400, the position buffer (VPS₋₋ POSITION₋₋ QUEUE) 5332. The task sendsa message to the "main" 5316 whenever a position fault occurs.

h. Vps₋₋ posture

This task 5324 is shown at the lower left-hand corner of the taskdiagram. When the vehicle is tracking, this task maintains the posturebuffer (VPS₋₋ POSTURE₋₋ QUEUE) 5334. The task (5324) monitors thevehicle's position and maintains approximately 50 postures, from thecurrent vehicle position in the direction of travel, in the posturebuffer (3000).

i. Tracker

Shown in the upper right-hand corner of the task diagram FIG. 53, thetask 5306 reads the current position 5332 and posture buffers 5334.Based on the information read, task 5306 calculates steer and speedcorrections 420. It sends them to the vehicle 310, thereby controllingthe vehicle's course.

j. Navigator Shared (Global) Memory

As mentioned above with regard to the navigator tasks 5300, thenavigator 406 has a global memory structure 5400 which the various tasksread/write. This memory structure 5400 is illustrated in FIG. 54.

Referring now to FIG. 54, the tasks are depicted as ellipsoids, with theparticular task written inside. The memory 5400 is depicted in thecenter section of FIG. 54 as a stack of boxes. Unprotected memory isdepicted as a single box in the stack of boxes. Semaphore protectedmemory is depicted as a box within a box in the stack.

An arrow points in the direction of data transfer between tasks andmemory. Therefore, a write to memory from a task is shown as a line withan arrow pointing towards the memory in question from the task.Likewise, a read from memory by a task is depicted by a line with anarrow pointing towards the task in question from the memory. Wheretwo-way data transfer between task and memory exists, a line with anarrow at both ends is shown.

k. Main (Exec) Flow Charts

FIGS. 55 and 56A-56D are flow charts of the navigator main or executivetask 5316.

Referring first to FIG. 55, it is a diagram of the general structure ofthe main or executive task flow. The following describes severalflowcharts associated with the navigator executive task 5316.

Referring to FIG. 55, which is the executive flowchart, it shows of fiveblocks: block 5502 which is the Start block; block 5504 which is theinitialize navigator; block 5506, which is the Pend on Exec Queue; block5506, which is the executive decisions; and block 5510, which is the acton state.

Flowchart FIG. 55 describes how the executive task 5316 executes itsfunctions beginning at power up (switching on electrical power) of thenavigator 406. Upon powerup, the executive task 5316 (or Executive)begins at the start block 5502 and proceeds immediately to initializenavigator 5504, where the executive 5316 puts the navigator 406 in aknown initial state. The executive then proceeds to the Pend on exec que5506 and waits for a message from a number of sources to arrive in itsmessage que 5328. For example, a typical message could be a query forinformation from the host computer 402.

Upon receipt of a message in the Exec Que 5328, the executive 5316proceeds to the executive decisions block 5508. In this block, theexecutive 5316 sets a series of status flags in a known manner. Theseflags put the navigator 406 in a known state, particular to the messagereceived.

Once the status flags have been properly set, the executive 5316 thenproceeds to the Act on State 5510, where the necessary action is carriedout according to the type of instruction received.

Referring now to FIGS. 56A-56D, they show the flow of the "executivedecisions" block 5508 of the general structure diagram FIG. 55.

The various responses which the executive task 5316 can initiate are nowdescribed in more detail. There are a known set of messages which areexpected within the Exec Que 5328. These messages are shown in detail inFIGS. 56A-56D.

FIG. 56A diagrams the organization of FIGS. 56A-56D. FIGS. 56A-56Ddescribe in detail the procedure which the executive 5316 uses torespond to various messages.

Referring to FIG. 56A, the action of the executive 5316 to particularmessages is described. Upon receipt of a message to the Exec Que 5328,the program flow leaves block 5506 and proceeds to block 5602, where theexecutive 5316 determines if the message is `NEW ROUTE DIRECTIVE`. Ifthe message is `NEW₋₋ ROUTE₋₋ DIRECTIVE`, then the executive 5316proceeds to the Act On "NEW₋₋ ROUTE₋₋ DIRECTIVE` block 5604. Once theaction particular to the `NEW₋₋ ROUTE₋₋ DIRECTIVE` message has beencompleted successfully, the executive 5316 then proceeds to the Act onState block 5510. Once the action has been completed, the executive 5316returns to the Pend on Exec Que block 5506 to await another message. Ifthe initial message in block 5602 is not `NEW₋₋ ROUTE₋₋ DIRECTIVE`, thenthe executive 5316 proceeds to block 5606 to determine if the message is`CHANGE₋₋ SPEED₋₋ DIRECTIVE`.

The response to messages such as `CHANGE₋₋ SPEED₋₋ DIRECTIVE`, `VEH₋₋RESPONDING`, NO₋₋ VEH₋₋ RESPONSE`, and VEH₋₋ CHECKSUM₋₋ ERR`, follow aprocedure similar to that described for the message `NEW₋₋ ROUTE₋₋DIRECTIVE`. However, the actions performed in the Act On `. . . ` blocks5604 through 5620 are different for the different possible messages. Thevarious types of valid messages and a brief description of each are:

NEW₋₋ ROUTE₋₋ DIRECTIVE: set the route number for the vehicle to follow.

CHANGE₋₋ SPEED DIRECTIVE: command a maximum possible speed for which thevehicle can traverse a particular part of the route.

VEH₋₋ RESPONDING: the vehicle is responding to commands properly, setNavigator status flags to Healthy.

NO₋₋ VEH₋₋ RESPONSE: the vehicle is not responding to commands, stop thevehicle.

VEH₋₋ CHECKSUM₋₋ ERR: the vehicle is not receiving/sensing datacorrectly, stop the vehicle.

TELE, MANUAL, READY, or AUTO: set the mode of the vehicle IN THE PROPERORDER.

VPS₋₋ TIMOUT: VPS is not sending data, stop the vehicle.

VPS₋₋ CHECKSUM₋₋ ERROR: the VPS is sending garbled data, stop thevehicle.

VPS₋₋ POSTURE₋₋ READY: ready to generate path postures.

VPS₋₋ POSITION₋₋ READY: VPS data is available.

VPS₋₋ POSITION₋₋ ALIGN: the VPS system is initializing, do not move thevehicle.

END₋₋ OF₋₋ ROUTE: the vehicle is approaching the end of the currentroute, has been reached inform the host computer.

SCAN₋₋ READY: the scanning system is ready look for objects in the path.

SCAN₋₋ ALL₋₋ CLEAR: no objects have been detected in the vehicle path,continue normally.

SCAN₋₋ OBSTACLE: an object has been detected o n the vehicle path, stopthe vehicle.

TRACKER₋₋ OFF₋₋ COURSE: the vehicle is not following the desired pathwithin tolerance, stop the vehicle.

TRACKER₋₋ END₋₋ OF₋₋ ROUTE: tracker has reached the end of the path,stop the vehicle.

TRACKER₋₋ STOPPED: notify the Navigator that the tracking task hasstopped the vehicle.

The responses to the messages `TELE`, `MANUAL`, `AUTO`, and `READY` aresomewhat different because these messages are related a nd mus t beacted upon in a specific order. This has been described above. Theprogram flow for these messages is shown in FIGS. 56A and 56B withrespect to block 5622-5630.

The response to subsequent message possibilities are depicted by blocks5632 through 5678 in FIGS. 56B through 56D. These responses are similarto those described for the message `NEW₋₋ ROUTE₋₋ DIRECTIVE`.

If the received message is not one of the expected messages, or if themessage is garbled, then the executive 5316 is directed to block 5680,where the host computer 402 is informed of the problem. The executive5316 then returns to the Exec Que 5506 to respond to the next message inthe queue.

FIGS. 57A through 57R show the specific procedures which the executive5316 uses to respond to a particular message. For example, FIG. 57Adetails how the executive 5316 responds to a `NEW₋₋ ROUTE₋₋ DIRECTIVE`message. Once this message arrives in the Exec Que 5328, the executive5316 then proceeds to a block 5702 to determine what the message is: inthis case, `NEW₋₋ ROUTE₋₋ DIRECTIVE`. If the message is `NEW₋₋ ROUTE₋₋DIRECTIVE`, the executive 5316 then proceeds to a block 5705 to respondto the message. Otherwise, it proceeds on to a block 5704 to determineif it is valid (one of the other possible messages) or invalid.

Given that the message is a `NEW₋₋ ROUTE₋₋ DIRECTIVE`, then theexecutive 5316 follows the process described in FIG. 57A to respond tothe message. This process is depicted by blocks 5706 through 5714. Inthis procedure (and in responses to other directives), the executive5316 checks the states of different tasks within the navigator 406 andreacts to these states in a known, predetermined manner.

The effect of this response is to set a series of status flags, whicheffect subsequent responses by other tasks in the navigator 406 when theexecutive 5316 reaches the Act On State 5510. The actual proceduresimplemented in block 5510 is shown in FIG. 58.

The responses of the executive 5316 to other valid messages are similarto that described for the `NEW₋₋ ROUTE₋₋ DIRECTIVE`. The effect of eachresponse to directives first changes a set of flags, which in turnaffect the state of the navigator 406. The particular flags set dependon the particular directive. The navigator 406 responds to the changesin these flags when the executive 5316 moves to the Act On State block5510.

FIGS. 58A-58C illustrate the flow of the "act on state" block 5510.

The act on state block 5510 is shown in FIGS. 58A-58C. FIG. 58 shows theinterrelationship of FIGS. 58A-58C, where each of these three figuresdepict a portion of the Act On State block 5510.

Once the executive task 5316 has set the appropriate flags in responseto a particular Exec message, the executive 5316 then proceeds to sendmessages to the appropriate tasks or entities which must be informed ofchanges to the navigator 406 system as a result of the Exec message.

For example, when the executive task 5316 leaves the executive decisions5508, and first enters the act on state block 5510, it checks to seethat the status is set such that the vehicle is ready for autonomousmode (for example--VPS is ready, vehicle is communicating properly, aproper route has been commanded, and the vehicle is ready for automode). See block 5802. If one or more of these conditions is not met,then the Exec returns to wait for another valid message. If all of theseconditions are met, then the executive 5316 checks to see that the pathgenerator 5804 is operating. If so, then the executive 5316 proceeds tostart the other systems required for autonomous operation.

If the path generation system is not operating, then the executive 5316task sends the message `VPS₋₋ POSTURE₋₋ ENGAGE` to the Vps Posture Queue5334, in order to start the path generator. The executive task will thenreturn to the Pend on Exec Queue 5506, to wait for another directive sothat proper operation of the vehicle 310 is ensured.

What is claimed is:
 1. A method for controlling the navigation of asurface based vehicle over a route, the method including:manuallydriving the vehicle over the route to collect driven data comprising theabsolute position of the vehicle along the route relative to the earth'ssurface; smoothing out said driven data to produce smoothed datacomprising a consistent path; and using said smoothed data toautomatically navigate the vehicle over the route.
 2. The navigationmethod of claim 1, wherein said step of smoothing out said driven dataincludes the step of:integrating said driven data to smooth said drivendata so that measured vehicle position, heading and curvature areconsistent along the route.
 3. A system for controlling the navigationof a surface based vehicle of the type having an instantaneous center towhich all motions can be referenced, exhibiting either oversteer orundersteer characteristics over a route, comprising:means for manuallydriving the vehicle over the route to collect driven data comprising theabsolute position of the vehicle along the route relative to the earth'ssurface; means for smoothing out said driven data to produce smootheddata comprising a consistent path; and means for using said smootheddata to automatically navigate the vehicle over the route.
 4. Thenavigation system of claim 3, wherein said means for smoothing out saiddriven data further comprises:means for integrating said driven data tosmooth said driven data so that measured vehicle position, heading andcurvature are consistent along the route.